diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 8bcb8d40c1c4..4e61920c58ee 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -9,11 +9,10 @@ pipeline { script { def build_nodes = [:] def docker_images = [ - armhf: "px4io/px4-dev-armhf:2021-02-04", - arm64: "px4io/px4-dev-aarch64:2021-02-04", - base: "px4io/px4-dev-base-bionic:2021-02-04", - nuttx: "px4io/px4-dev-nuttx-focal:2021-02-04", - snapdragon: "lorenzmeier/px4-dev-snapdragon:2020-04-01" + armhf: "px4io/px4-dev-armhf:2021-09-08", + arm64: "px4io/px4-dev-aarch64:2021-09-08", + base: "px4io/px4-dev-base-bionic:2021-09-08", + nuttx: "px4io/px4-dev-nuttx-focal:2021-09-08", ] def armhf_builds = [ @@ -29,7 +28,7 @@ pipeline { ] def base_builds = [ - target: ["px4_sitl_rtps"], + target: ["px4_sitl_default"], image: docker_images.base, archive: false ] @@ -39,6 +38,11 @@ pipeline { "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", + "atl_mantis-edu_default", "av_x-v1_default", "bitcraze_crazyflie_default", "bitcraze_crazyflie21_default", @@ -46,21 +50,33 @@ pipeline { "cuav_can-gps-v1_default", "cuav_nora_default", "cuav_x7pro_default", - "cubepilot_cubeorange_console", "cubepilot_cubeorange_default", - "cubepilot_cubeyellow_console", "cubepilot_cubeyellow_default", + "diatone_mamba-f405-mk2_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_pix32v5_default", + "matek_gnss-m9n-f4_canbootloader", + "matek_gnss-m9n-f4_default", + "matek_h743-mini_default", + "matek_h743-slim_default", + "matek_h743_default", "modalai_fc-v1_default", + "modalai_fc-v1_rtps", + "modalai_fc-v2_default", "mro_ctrl-zero-f7_default", "mro_ctrl-zero-f7-oem_default", - "mro_ctrl-zero-h7_default", "mro_ctrl-zero-h7-oem_default", + "mro_ctrl-zero-h7-oem_rtps", + "mro_ctrl-zero-h7_default", + "mro_ctrl-zero-h7_rtps", "mro_pixracerpro_default", + "mro_pixracerpro_rtps", "mro_x21-777_default", "mro_x21_default", "nxp_fmuk66-e_default", @@ -70,35 +86,31 @@ pipeline { "nxp_fmuk66-v3_rtps", "nxp_fmuk66-v3_socketcan", "nxp_fmurt1062-v1_default", - "nxp_ucans32k146_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-v2_test", "px4_fmu-v3_default", - "px4_fmu-v4_cannode", "px4_fmu-v4_default", "px4_fmu-v4pro_default", - "px4_fmu-v5_ctrlalloc", + "px4_fmu-v5_cyphal", "px4_fmu-v5_debug", "px4_fmu-v5_default", - "px4_fmu-v5_fixedwing", - "px4_fmu-v5_multicopter", - "px4_fmu-v5_optimized", - "px4_fmu-v5_rover", + "px4_fmu-v5_lto", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", - "px4_fmu-v5_uavcanv1", - "px4_fmu-v5x_base_phy_DP83848C", "px4_fmu-v5x_default", + "px4_fmu-v6c_default", "px4_fmu-v6u_default", - "px4_fmu-v6u_test", "px4_fmu-v6x_default", "px4_io-v2_default", + "raspberrypi_pico_default", + "sky-drones_smartap-airlink_default", "spracing_h7extreme_default", "uvify_core_default" ], @@ -106,14 +118,8 @@ pipeline { archive: true ] - def snapdragon_builds = [ - target: ["atlflight_eagle_qurt", "atlflight_eagle_default"], - image: docker_images.snapdragon, - archive: false - ] - def docker_builds = [ - armhf_builds, base_builds, nuttx_builds_archive//, snapdragon_builds + armhf_builds, base_builds, nuttx_builds_archive ] for (def build_type = 0; build_type < docker_builds.size(); build_type++) { @@ -133,7 +139,7 @@ pipeline { // TODO: actually upload artifacts to S3 // stage('S3 Upload') { // agent { - // docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + // docker { image 'px4io/px4-dev-base-focal:2021-09-08' } // } // options { // skipDefaultCheckout() @@ -158,18 +164,14 @@ pipeline { } options { buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14')) - timeout(time: 60, unit: 'MINUTES') + timeout(time: 90, unit: 'MINUTES') } } def createBuildNode(Boolean archive, String docker_image, String target) { return { - // TODO: fix the snapdragon image bypass_entrypoint = '' - if (docker_image == 'lorenzmeier/px4-dev-snapdragon:2020-04-01') { - bypass_entrypoint = ' --entrypoint=""' - } node { docker.withRegistry('https://registry.hub.docker.com', 'docker_hub_dagar') { @@ -178,7 +180,7 @@ def createBuildNode(Boolean archive, String docker_image, String target) { try { sh('export') checkout(scm) - sh('make distclean') + sh('make distclean; git clean -ff -x -d .') sh('git fetch --tags') sh('ccache -s') sh('make ' + target) @@ -195,7 +197,7 @@ def createBuildNode(Boolean archive, String docker_image, String target) { throw (exc) } finally { - sh('make distclean') + sh('make distclean; git clean -ff -x -d .') } } } diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index e37c83d5a3f2..ba01d04b3f01 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -7,33 +7,29 @@ pipeline { parallel { - stage("cubepilot_cubeorange_console") { + stage("cubepilot_cubeorange_test") { stages { - stage("build cubepilot_cubeorange_console") { + stage("build cubepilot_cubeorange_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' + checkoutSCM() + sh 'make cubepilot_cubeorange_bootloader' + sh 'make cubepilot_cubeorange_test' sh 'ccache -s' - sh 'git fetch --tags' - sh 'make cubepilot_cubeorange_console' - sh 'make sizes' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cubepilot_cubeorange_console' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'cubepilot_cubeorange' } @@ -42,75 +38,73 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'cubepilot_cubeorange_console' + 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_console/cubepilot_cubeorange_console.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 2000"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "px4io forceupdate 14662 /etc/extras/cubepilot_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply + // 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 { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + 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_default") { + stage("cuav_x7pro_test") { stages { - stage("build cuav_x7pro_default") { + stage("build cuav_x7pro_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make cuav_x7pro_default' - sh 'make sizes' + checkoutSCM() + sh 'make cuav_x7pro_bootloader' + sh 'make cuav_x7pro_test' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cuav_x7pro_default' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'cuav_x7pro' } @@ -119,126 +113,68 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'cuav_x7pro_default' + 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_default/cuav_x7pro_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 2000"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + 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-v2_test") { - // stages { - // stage("build px4_fmu-v2_test") { - // agent { - // docker { - // image 'px4io/px4-dev-nuttx-focal:2020-09-14' - // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - // } - // } - // steps { - // sh 'export' - // sh 'make distclean' - // sh 'ccache -s' - // sh 'git fetch --tags' - // sh 'make px4_fmu-v2_test' - // sh 'make sizes' - // sh 'ccache -s' - // stash includes: 'build/px4_fmu-v2_test/px4_fmu-v2_test.elf', name: 'px4_fmu-v2_test' - // } - // post { - // always { - // sh 'make distclean' - // } - // } - // } // stage build - // stage("test") { - // agent { - // label 'px4_fmu-v2' - // } - // stages { - // stage("flash") { - // steps { - // sh 'export' - // sh 'find /dev/serial' - // unstash 'px4_fmu-v2_test' - // // flash board and watch bootup - // sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v2_test/px4_fmu-v2_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - // } - // } - // stage("reset") { - // steps { - // cleanupFTDI(); - // } - // } - // } - // options { - // timeout(time: 90, unit: 'MINUTES') - // } - // } - // } - // } - - stage("px4_fmu-v3_default") { + stage("px4_fmu-v3_test") { stages { - stage("build px4_fmu-v3_default") { + stage("build px4_fmu-v3_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make px4_fmu-v3_default' - sh 'make sizes' + checkoutSCM() + sh 'make px4_fmu-v3_test' + sh 'make px4_fmu-v3_test bootloader_elf' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v3_default' + stash includes: 'build/*/*.elf, platforms/nuttx/Debug/*, platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit, Tools/HIL/*', name: 'px4_fmu-v3_test' } post { always { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'px4_fmu-v3' } @@ -247,74 +183,68 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v3_default' + unstash 'px4_fmu-v3_test' + sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v3_test/px4_fmu-v3_bootloader.elf' // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v3_default/px4_fmu-v3_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + resetBoard() } } - stage("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 13000"' // generic vtol standard - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + post { + always { + sh 'cat /tmp/pyserial_spy_file.txt || true' + sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true' + } } } // stage test } } - stage("px4_fmu-v4_default") { + stage("px4_fmu-v4_test") { stages { - stage("build px4_fmu-v4_default") { + stage("build px4_fmu-v4_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' + checkoutSCM() + sh 'make px4_fmu-v4_test' + sh 'make px4_fmu-v4_test bootloader_elf' sh 'ccache -s' - sh 'git fetch --tags' - sh 'make px4_fmu-v4_default' - sh 'make sizes' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v4_default' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'px4_fmu-v4' } @@ -323,73 +253,67 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v4_default' + 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_default/px4_fmu-v4_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + 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_default") { + stage("px4_fmu-v4pro_test") { stages { - stage("build px4_fmu-v4pro_default") { + stage("build px4_fmu-v4pro_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make px4_fmu-v4pro_default' - sh 'make sizes' + 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/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v4pro_default' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'px4_fmu-v4pro' } @@ -398,119 +322,41 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v4pro_default' + 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_default/px4_fmu-v4pro_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 13000"' // generic vtol standard - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') - } - } // stage test - } - } - - stage("px4_fmu-v5_default") { - stages { - stage("build px4_fmu-v5_default") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make px4_fmu-v5_default' - sh 'make sizes' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_default' - } post { always { - sh 'make distclean' + 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 build - stage("test") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_default' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_default/px4_fmu-v5_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - } - } - stage("configure") { - steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply - } - } - stage("status") { - steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' - } - } - stage("reset") { - steps { - cleanupFTDI(); - } - } - } - options { - timeout(time: 90, unit: 'MINUTES') - } } // stage test } } @@ -520,28 +366,24 @@ pipeline { stage("build px4_fmu-v5_debug") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' + checkoutSCM() sh 'make px4_fmu-v5_debug' - sh 'make sizes' + sh 'make px4_fmu-v5_debug bootloader_elf' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_debug' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'px4_fmu-v5' } @@ -550,121 +392,61 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 250" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' 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' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 250"' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // 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 "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"' + + 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 { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') - } - } // stage test - } - } - - stage("px4_fmu-v5_optimized") { - stages { - stage("build px4_fmu-v5_optimized") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make px4_fmu-v5_optimized' - sh 'make sizes' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_optimized' - } post { always { - sh 'make distclean' + 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 build - stage("test") { - agent { - label 'px4_fmu-v5' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v5_optimized' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_optimized/px4_fmu-v5_optimized.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - } - } - stage("configure") { - steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // reboot to apply - } - } - stage("status") { - steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' - } - } - stage("reset") { - steps { - cleanupFTDI(); - } - } - } - options { - timeout(time: 90, unit: 'MINUTES') - } } // stage test } } @@ -674,28 +456,24 @@ pipeline { stage("build px4_fmu-v5_stackcheck") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' + checkoutSCM() sh 'make px4_fmu-v5_stackcheck' - sh 'make sizes' + sh 'make px4_fmu-v5_stackcheck bootloader_elf' sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_stackcheck' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'px4_fmu-v5' } @@ -704,232 +482,150 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 250" || true' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true' 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 250"' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0"' // limit cpu usage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // 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 "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"' } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') - } - } // stage test - } - } - - stage("modalai_fc-v1_default") { - stages { - stage("build modalai_fc-v1_default") { - agent { - docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' - } - } - steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make modalai_fc-v1_default' - sh 'make sizes' - sh 'ccache -s' - stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_default' - } post { always { - sh 'make distclean' - } - } - } // stage build - stage("test") { - agent { - label 'modalai_fc-v1' - } - stages { - stage("flash") { - steps { - sh 'export' - sh 'find /dev/serial' - unstash 'modalai_fc-v1_default' - // flash board and watch bootup - sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_default/modalai_fc-v1_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' - } - } - stage("configure") { - steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic vtol standardulticopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // reboot to apply - } - } - stage("status") { - steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' - } - } - stage("reset") { - steps { - cleanupFTDI() - } + 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' } } - options { - timeout(time: 90, unit: 'MINUTES') - } } // stage test } } - stage("holybro_durandal-v1_default") { + stage("px4_fmu-v5_test") { stages { - stage("build holybro_durandal-v1_default") { + stage("build px4_fmu-v5_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make holybro_durandal-v1_default' - sh 'make sizes' + 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/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'holybro_durandal-v1_default' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { - label 'holybro_durandal-v1' + label 'px4_fmu-v5' } stages { stage("flash") { steps { sh 'export' sh 'find /dev/serial' - unstash 'holybro_durandal-v1_default' + 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/holybro_durandal-v1_default/holybro_durandal-v1_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set DSHOT_CONFIG 600"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 4000"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_USE_IO 0"' // for dshot - 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 "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dshot status"' - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`' + // 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 + checkStatus() + quickCalibrate() + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true' } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + 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_default") { + stage("nxp_fmuk66-v3_test") { stages { - stage("build nxp_fmuk66-v3_default") { + stage("build nxp_fmuk66-v3_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2020-09-14' - args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + image 'px4io/px4-dev-nuttx-focal:2021-09-08' + args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { - checkout scm - sh 'export' - sh 'make distclean' - sh 'ccache -s' - sh 'git fetch --tags' - sh 'make nxp_fmuk66-v3_default' - sh 'make sizes' + 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/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'nxp_fmuk66-v3_default' + 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' + sh 'make distclean; git clean -ff -x -d .' } } } // stage build - stage("test") { + stage("hardware") { agent { label 'nxp_fmuk66-v3' } @@ -938,41 +634,40 @@ pipeline { steps { sh 'export' sh 'find /dev/serial' - unstash 'nxp_fmuk66-v3_default' + 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_default/nxp_fmuk66-v3_default.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600' + 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("configure") { + stage("tests") { steps { - // configure - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_BL_UPDATE 1"' // update bootloader - 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 "reboot" || true' // reboot to apply + runTests() } } stage("status") { steps { - statusFTDI() - } - } - stage("tests") { - steps { - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*` || true' // allow failure due to intermittent serial console issues + // 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 + checkStatus() + quickCalibrate() } } - stage("reset") { + stage("print topics") { steps { - cleanupFTDI(); + printTopics() } } } - options { - timeout(time: 90, unit: 'MINUTES') + 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 } @@ -983,129 +678,282 @@ pipeline { } // stages environment { CCACHE_DIR = '/tmp/ccache' + CCACHE_NOHASHDIR = 1 CI = true } options { - buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '40')) - timeout(time: 90, unit: 'MINUTES') + buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60')) + timeout(time: 180, unit: 'MINUTES') skipDefaultCheckout() } } -void statusFTDI() { +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 "param dump"' + 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 "param show SYS*"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump"' + // run logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sleep 1"' // sleep before continuing // status commands - 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 "work_queue status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "board_adc test"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander check"' + 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 "commander calibrate accel quick; sleep 2; param show CAL_ACC*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 2; param show CAL_MAG*"' 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 "dmesg"' + 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 "gyro_calibration status; param show CAL_GYRO*"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener px4io_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags"' + 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 "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 "mavlink status streams"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status"' + 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 "param show"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump"' + 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 info -d /dev/pwm_output0"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm info -d /dev/pwm_output1"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm info -d /dev/pwm_output1" || true' 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 "px4io status"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -r 2"' 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 "listener cpuload; top once; listener cpuload"' - 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"' + 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"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status; param show CAL_GYRO*"' - 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"' // stop logger sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' } -void cleanupFTDI() { - 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_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman stop"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"' +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 10016' + + 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 "ostest"' + sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot after ostest - // wipe sdcard - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "umount /fs/microsd"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mkfatfs /dev/mmcsd0"' + 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' - // drop any uncommited hardfaults - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "hardfault_log rearm"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "hardfault_log reset"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' - // erase mtd + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param dump /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 "param dump /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 "param dump /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 "param dump /fs/mtd_params"' - // disable buzzer and cleanup storage - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param reset_all"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_DIRS_MAX 1"' - 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 "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_controls_3" || 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 test_motor" || 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_acceleration" || 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 vehicle_status_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() - // reboot - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' + 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/.clang-tidy b/.clang-tidy index 649cd6d8ac9f..2f9fdf276534 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -74,6 +74,7 @@ Checks: '*, -modernize-deprecated-headers, -modernize-loop-convert, -modernize-pass-by-value, + -modernize-raw-string-literal, -modernize-return-braced-init-list, -modernize-use-auto, -modernize-use-bool-literals, @@ -81,6 +82,7 @@ Checks: '*, -modernize-use-equals-default, -modernize-use-equals-delete, -modernize-use-override, + -modernize-use-trailing-return-type, -modernize-use-using, -performance-inefficient-string-concatenation, -readability-avoid-const-params-in-decls, diff --git a/.clusterfuzzlite/Dockerfile b/.clusterfuzzlite/Dockerfile new file mode 100644 index 000000000000..181031c69581 --- /dev/null +++ b/.clusterfuzzlite/Dockerfile @@ -0,0 +1,7 @@ +FROM gcr.io/oss-fuzz-base/base-builder:v1 +COPY . $SRC/PX4-Autopilot +RUN apt-get install -y libjpeg8-dev zlib1g-dev +RUN pip3 install --upgrade pip +RUN python3 -m pip install -r $SRC/PX4-Autopilot/Tools/setup/requirements.txt +WORKDIR $SRC/PX4-Autopilot +COPY ./.clusterfuzzlite/build.sh $SRC/ diff --git a/.clusterfuzzlite/build.sh b/.clusterfuzzlite/build.sh new file mode 100644 index 000000000000..ac0892c59f60 --- /dev/null +++ b/.clusterfuzzlite/build.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash -eu + +PX4_FUZZ=1 make px4_sitl +cp build/px4_sitl_default/bin/px4 $OUT/px4 diff --git a/.clusterfuzzlite/project.yaml b/.clusterfuzzlite/project.yaml new file mode 100644 index 000000000000..b4788012b102 --- /dev/null +++ b/.clusterfuzzlite/project.yaml @@ -0,0 +1 @@ +language: c++ diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index eb2dc80d1fc6..b0914106edcc 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp { "name": "px4-dev-nuttx", - "image": "px4io/px4-dev-nuttx-focal:2021-02-04", + "image": "px4io/px4-dev-nuttx-focal:2021-09-08", "runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ], diff --git a/.github/ISSUE_TEMPLATE/1_Bug_report.md b/.github/ISSUE_TEMPLATE/1_Bug_report.md index 128e17663c6f..80758e0190db 100644 --- a/.github/ISSUE_TEMPLATE/1_Bug_report.md +++ b/.github/ISSUE_TEMPLATE/1_Bug_report.md @@ -1,32 +1,34 @@ --- -name: Bug report +name: 🐛 Bug report about: Create a report to help us improve +labels: bug-report --- -**Describe the bug** +## Describe the bug A clear and concise description of the bug. -**To Reproduce** +## To Reproduce Steps to reproduce the behavior: 1. Drone switched on '...' 2. Uploaded mission '....' (attach QGC mission file) 3. Took off '....' 4. See error -**Expected behavior** +## Expected behavior A clear and concise description of what you expected to happen. -**Log Files and Screenshots** +## Log Files and Screenshots *Always* provide a link to the flight log file: - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/en/getting_started/flight_reporting.html)). -- Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/). +- Upload the log to the [PX4 Flight Review](http://logs.px4.io/) +- Share the link to the log (Copy and paste the URL of the log) Add screenshots to help explain your problem. -**Drone (please complete the following information):** +## Drone (please complete the following information): - Describe the type of drone. - Photo of the IMU / autopilot setup if possible. -**Additional context** +## Additional context Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/2_Feature_request.md b/.github/ISSUE_TEMPLATE/2_Feature_request.md index c55e5a62db36..754f3fd4cc57 100644 --- a/.github/ISSUE_TEMPLATE/2_Feature_request.md +++ b/.github/ISSUE_TEMPLATE/2_Feature_request.md @@ -1,19 +1,20 @@ --- name: 🚀 Feature Request about: Suggest an idea for this project +labels: feature-request --- -For general questions please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/). +For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README). -**Describe problem solved by the proposed feature** +## Describe problem solved by the proposed feature A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ... -**Describe your preferred solution** +## Describe your preferred solution A clear and concise description of what you want to happen. -**Describe possible alternatives** +## Describe possible alternatives A clear and concise description of alternative solutions or features you've considered. -**Additional context** +## Additional context Add any other context or screenshots for the feature request here. diff --git a/.github/ISSUE_TEMPLATE/3_Support_question.md b/.github/ISSUE_TEMPLATE/3_Support_question.md index 9b69773f9b95..7c331f2dc17d 100644 --- a/.github/ISSUE_TEMPLATE/3_Support_question.md +++ b/.github/ISSUE_TEMPLATE/3_Support_question.md @@ -1,10 +1,13 @@ --- name: ⛔ Support Question -about: See [PX4 Discuss](http://discuss.px4.io/) for questions about using PX4. +about: See http://discuss.px4.io/ for questions about using PX4. --- -We use GitHub issues only to discuss PX4 bugs and new features. For -questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/). +## Attention! Please read the note below + +We use GitHub issues only to discuss PX4 bugs and new features. + +**For questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).** Thanks! diff --git a/.github/ISSUE_TEMPLATE/4_Documentation_issue.md b/.github/ISSUE_TEMPLATE/4_Documentation_issue.md index 42c0e77772eb..0e14e82cc564 100644 --- a/.github/ISSUE_TEMPLATE/4_Documentation_issue.md +++ b/.github/ISSUE_TEMPLATE/4_Documentation_issue.md @@ -1,9 +1,11 @@ --- name: ⛔ Documentation Issue -about: See https://github.com/PX4/Devguide for documentation issues +about: See https://github.com/PX4/px4_user_guide for documentation issues --- -PX4 has dedicated repositories for developer documentation (https://github.com/PX4/Devguide) and user documentation (https://github.com/PX4/px4_user_guide). +## Attention! Please read the note below + +**Please submit the documentation issue to the [User Guide](https://github.com/PX4/px4_user_guide) repository.** Thanks! diff --git a/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md similarity index 75% rename from PULL_REQUEST_TEMPLATE.md rename to .github/PULL_REQUEST_TEMPLATE.md index 47ecbb7c1771..7c68bee3411f 100644 --- a/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,17 +1,17 @@ Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback. -**Describe problem solved by this pull request** -A clear and concise description of the problem this proposed change will solve. +## Describe problem solved by this pull request +A clear and concise description of the problem this proposed change will solve. Or, what it will improve. E.g. For this use case I ran into... -**Describe your solution** +## Describe your solution A clear and concise description of what you have implemented. -**Describe possible alternatives** +## Describe possible alternatives A clear and concise description of alternative solutions or features you've considered. -**Test data / coverage** +## Test data / coverage How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts. -**Additional context** +## Additional context Add any other related context or media. diff --git a/.github/slack.svg b/.github/slack.svg new file mode 100644 index 000000000000..2b0fbae4aa97 --- /dev/null +++ b/.github/slack.svg @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + slack + slack + Join us! + Join us! + + + diff --git a/.github/workflows/cflite_batch.yml b/.github/workflows/cflite_batch.yml new file mode 100644 index 000000000000..d1321cc6b7fb --- /dev/null +++ b/.github/workflows/cflite_batch.yml @@ -0,0 +1,34 @@ +name: ClusterFuzzLite batch fuzzing +on: + schedule: + - cron: '0 6 * * *' # UTC 6am every day. +permissions: read-all +jobs: + BatchFuzzing: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + sanitizer: + - address + - undefined + - memory + steps: + - name: Build Fuzzers (${{ matrix.sanitizer }}) + id: build + uses: google/clusterfuzzlite/actions/build_fuzzers@v1 + with: + sanitizer: ${{ matrix.sanitizer }} + - name: Run Fuzzers (${{ matrix.sanitizer }}) + id: run + uses: google/clusterfuzzlite/actions/run_fuzzers@v1 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} + fuzz-seconds: 1800 # 30 mins + mode: 'batch' + sanitizer: ${{ matrix.sanitizer }} + # Optional but recommended: For storing certain artifacts from fuzzing. + # See later section on "Git repo for storage". + # storage-repo: https://${{ secrets.PERSONAL_ACCESS_TOKEN }}@github.com/OWNER/STORAGE-REPO-NAME.git + # storage-repo-branch: main # Optional. Defaults to "main" + # storage-repo-branch-coverage: gh-pages # Optional. Defaults to "gh-pages". diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index a9c93ecc3b03..bcae79ad6951 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -23,39 +23,19 @@ jobs: "shellcheck_all", "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", + "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps", "airframe_metadata", "module_documentation", "parameters_metadata", ] container: - image: px4io/px4-dev-nuttx-focal:2021-02-04 + image: px4io/px4-dev-nuttx-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 with: token: ${{ secrets.ACCESS_TOKEN }} - - 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 - with: - path: ~/.ccache - key: tests_${{matrix.ubuntu_release}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: tests_${{matrix.ubuntu_release}}-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 = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - name: check environment run: | export diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index 19d3bfee318c..69569170c890 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -11,36 +11,11 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-clang:2021-02-04 + container: px4io/px4-dev-clang:2021-09-08 steps: - uses: actions/checkout@v1 with: token: ${{secrets.ACCESS_TOKEN}} - - 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 - with: - path: ~/.ccache - key: clang-tidy-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: clang-tidy-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 = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - name: make clang-tidy-quiet - run: | - ccache -z - make clang-tidy-quiet - ccache -s + run: make clang-tidy-quiet diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml index d15ecfb463b5..54ed88d9135f 100644 --- a/.github/workflows/compile_linux.yml +++ b/.github/workflows/compile_linux.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2021-02-04 + container: px4io/px4-dev-armhf:2021-09-08 strategy: matrix: config: [ @@ -42,8 +42,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.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 diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml index 68877079c560..5aef9adf273f 100644 --- a/.github/workflows/compile_linux_arm64.yml +++ b/.github/workflows/compile_linux_arm64.yml @@ -11,7 +11,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2021-02-04 + container: px4io/px4-dev-aarch64:2021-09-08 strategy: matrix: config: [ @@ -39,8 +39,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.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 diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 83764400f940..9be5fc8977fa 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -43,8 +43,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf + echo "compression_level = 6" >> ~/.ccache/ccache.conf + echo "max_size = 40M" >> ~/.ccache/ccache.conf + echo "hash_dir = false" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index f6f2482cac28..f871cd60a6eb 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -11,79 +11,63 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2021-02-04 + container: px4io/px4-dev-nuttx-focal:2021-09-08 strategy: + fail-fast: false matrix: config: [ - airmind_mindpx-v2_default, - ark_can-flow_canbootloader, - ark_can-flow_debug, - ark_can-flow_default, - av_x-v1_default, - bitcraze_crazyflie21_default, - bitcraze_crazyflie_default, - cuav_can-gps-v1_canbootloader, - cuav_can-gps-v1_debug, - cuav_can-gps-v1_default, - cuav_nora_default, - cuav_x7pro_default, - cubepilot_cubeorange_console, - cubepilot_cubeorange_default, - cubepilot_cubeyellow_console, - cubepilot_cubeyellow_default, - cubepilot_io-v2_default, - holybro_can-gps-v1_canbootloader, - holybro_can-gps-v1_debug, - holybro_can-gps-v1_default, - holybro_durandal-v1_default, - holybro_kakutef7_default, - holybro_pix32v5_default, - modalai_fc-v1_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_rtps, - nxp_fmuk66-e_socketcan, - nxp_fmuk66-v3_default, - nxp_fmuk66-v3_rtps, - nxp_fmuk66-v3_socketcan, - nxp_fmurt1062-v1_default, - nxp_ucans32k146_canbootloader, - nxp_ucans32k146_default, - omnibus_f4sd_default, - px4_fmu-v2_default, - px4_fmu-v2_fixedwing, - px4_fmu-v2_multicopter, - px4_fmu-v2_rover, - px4_fmu-v2_test, - px4_fmu-v3_default, - px4_fmu-v4_cannode, - px4_fmu-v4_default, - px4_fmu-v4pro_default, - px4_fmu-v5_ctrlalloc, - px4_fmu-v5_debug, - px4_fmu-v5_default, - px4_fmu-v5_fixedwing, - px4_fmu-v5_multicopter, - px4_fmu-v5_optimized, - px4_fmu-v5_rover, - px4_fmu-v5_rtps, - px4_fmu-v5_stackcheck, - px4_fmu-v5_uavcanv0periph, - px4_fmu-v5_uavcanv1, - px4_fmu-v5x_base_phy_DP83848C, - px4_fmu-v5x_default, - px4_fmu-v6u_default, - px4_fmu-v6u_test, - px4_fmu-v6x_default, - px4_io-v2_default, - spracing_h7extreme_default, - uvify_core_default + airmind_mindpx-v2, + ark_can-flow, + ark_can-gps, + ark_can-rtk-gps, + ark_cannode, + atl_mantis-edu, + av_x-v1, + bitcraze_crazyflie, + bitcraze_crazyflie21, + cuav_can-gps-v1, + cuav_nora, + cuav_x7pro, + cubepilot_cubeorange, + cubepilot_cubeyellow, + diatone_mamba-f405-mk2, + freefly_can-rtk-gps, + holybro_can-gps-v1, + holybro_durandal-v1, + holybro_kakutef7, + holybro_kakuteh7, + holybro_pix32v5, + matek_gnss-m9n-f4, + matek_h743, + matek_h743-mini, + matek_h743-slim, + modalai_fc-v1, + modalai_fc-v2, + mro_ctrl-zero-f7, + mro_ctrl-zero-f7-oem, + mro_ctrl-zero-h7, + mro_ctrl-zero-h7-oem, + mro_pixracerpro, + mro_x21, + mro_x21-777, + nxp_fmuk66-e, + nxp_fmuk66-v3, + nxp_fmurt1062-v1, + nxp_ucans32k146, + omnibus_f4sd, + px4_fmu-v2, + px4_fmu-v3, + px4_fmu-v4, + px4_fmu-v4pro, + px4_fmu-v5, + px4_fmu-v5x, + px4_fmu-v6c, + px4_fmu-v6u, + px4_fmu-v6x, + raspberrypi_pico, + sky-drones_smartap-airlink, + spracing_h7extreme, + uvify_core ] steps: - uses: actions/checkout@v1 @@ -107,13 +91,15 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf + echo "compression_level = 6" >> ~/.ccache/ccache.conf + echo "max_size = 120M" >> ~/.ccache/ccache.conf + echo "hash_dir = false" >> ~/.ccache/ccache.conf ccache -s ccache -z - - name: make ${{matrix.config}} - run: make ${{matrix.config}} + - name: make all_variants_${{matrix.config}} + run: make all_variants_${{matrix.config}} + timeout-minutes: 45 - name: make ${{matrix.config}} bloaty_compileunits run: make ${{matrix.config}} bloaty_compileunits || true - name: make ${{matrix.config}} bloaty_inlines @@ -124,13 +110,17 @@ jobs: run: make ${{matrix.config}} bloaty_symbols || true - name: make ${{matrix.config}} bloaty_templates run: make ${{matrix.config}} bloaty_templates || true + - name: make ${{matrix.config}} bloaty_ram + run: make ${{matrix.config}} bloaty_ram || true - name: make ${{matrix.config}} bloaty_compare_master run: make ${{matrix.config}} bloaty_compare_master || true - name: ccache post-run run: ccache -s - name: Upload px4 package - uses: actions/upload-artifact@v1 + uses: actions/upload-artifact@v2 with: name: px4_package_${{matrix.config}} - path: build/${{matrix.config}}/${{matrix.config}}.px4 + path: | + build/**/*.px4 + build/**/*.bin diff --git a/.github/workflows/compile_nuttx_cannode.yml b/.github/workflows/compile_nuttx_cannode.yml deleted file mode 100644 index 51acadf15090..000000000000 --- a/.github/workflows/compile_nuttx_cannode.yml +++ /dev/null @@ -1,61 +0,0 @@ -name: NuttX UAVCAN firmware - -on: - push: - branches: - - 'master' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2021-02-04 - strategy: - matrix: - config: [ - ark_can-flow_default, - cuav_can-gps-v1_default, - holybro_can-gps-v1_default, - #nxp_ucans32k146_default, - px4_fmu-v4_cannode - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - 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 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-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 = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - - name: ccache post-run - run: ccache -s - - - name: Upload px4 package - uses: actions/upload-artifact@v2 - with: - name: px4_cannode_${{matrix.config}} - path: build/${{matrix.config}}/*.uavcan.bin diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml index 51a7a859bbef..18480ac036ff 100644 --- a/.github/workflows/deploy_all.yml +++ b/.github/workflows/deploy_all.yml @@ -10,6 +10,7 @@ on: jobs: enumerate_targets: runs-on: ubuntu-latest + container: px4io/px4-dev-base-focal:2021-09-08 outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} steps: @@ -23,46 +24,22 @@ jobs: needs: enumerate_targets strategy: matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}} - container: px4io/px4-dev-${{ matrix.container }}:2021-02-04 + container: px4io/px4-dev-${{ matrix.container }}:2021-09-08 steps: - uses: actions/checkout@v1 with: token: ${{secrets.ACCESS_TOKEN}} - - 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 - with: - path: ~/.ccache - key: ${{matrix.target}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.target}}-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 = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - name: make ${{matrix.target}} run: make ${{matrix.target}} - - name: ccache post-run - run: ccache -s - - name: parameter metadata + - name: parameter & events metadata run: | - make ${{matrix.target}} ver_gen + 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.* _metadata + cp parameters.* events/*.xz actuators.json* _metadata - uses: jakejarvis/s3-sync-action@master with: diff --git a/.github/workflows/ekf_functional_change_indicator.yml b/.github/workflows/ekf_functional_change_indicator.yml new file mode 100644 index 000000000000..a5fa9db59955 --- /dev/null +++ b/.github/workflows/ekf_functional_change_indicator.yml @@ -0,0 +1,21 @@ +name: EKF Change Indicator + +on: pull_request + +jobs: + unit_tests: + runs-on: ubuntu-latest + container: px4io/px4-dev-base-focal:2021-09-08 + steps: + - uses: actions/checkout@v2.3.1 + 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 + - 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 new file mode 100644 index 000000000000..7de35207f096 --- /dev/null +++ b/.github/workflows/ekf_update_change_indicator.yml @@ -0,0 +1,29 @@ +name: EKF Update Change Indicator + +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 + with: + fetch-depth: 0 + - name: main test updates change indication files + run: 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 + 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' + 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 + run: exit 1 diff --git a/.github/workflows/mavros_avoidance_tests.yml b/.github/workflows/mavros_avoidance_tests.yml deleted file mode 100644 index 24bf94b3f4a0..000000000000 --- a/.github/workflows/mavros_avoidance_tests.yml +++ /dev/null @@ -1,131 +0,0 @@ -name: MAVROS Avoidance Tests - -on: - push: - branches: - - 'master' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - config: - - {test_file: "mavros_posix_test_avoidance.test", vehicle: "iris_obs_avoid", mission: "avoidance", build_type: "RelWithDebInfo"} - - {test_file: "mavros_posix_test_safe_landing.test", vehicle: "iris_obs_avoid", mission: "MC_safe_landing", build_type: "RelWithDebInfo"} - - container: - image: px4io/px4-dev-ros-melodic:2021-02-04 - options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined - steps: - - uses: actions/checkout@v1 - with: - token: ${{ secrets.ACCESS_TOKEN }} - - - 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 - 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 = 5" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: check environment - run: | - export - ulimit -a - - name: Build PX4 and sitl_gazebo - env: - PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: | - ccache -z - make px4_sitl_default - make px4_sitl_default sitl_gazebo - 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_avoidance_run.sh ${{matrix.config.test_file}} mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}} || true - - - 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@v2-preview - with: - name: coredump - path: px4.core - - # - name: ecl EKF analysis - # if: always() - # run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg - - - 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@v2 - with: - name: binary - path: build/px4_sitl_default/bin/px4 - - - name: Store PX4 log - if: failure() - uses: actions/upload-artifact@v2 - with: - name: px4_log - path: ~/.ros/log/*/*.ulg - - - name: Store ROS log - if: failure() - uses: actions/upload-artifact@v2 - 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_avoidance - file: coverage/lcov.info diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 60f158f24083..8401f61b1176 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -25,7 +25,7 @@ jobs: #- {vehicle: "tiltrotor", mission: "VTOL_mission_1", build_type: "RelWithDebInfo"} container: - image: px4io/px4-dev-ros-melodic:2021-02-04 + image: px4io/px4-dev-ros-melodic:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 @@ -49,8 +49,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.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 @@ -78,6 +79,7 @@ jobs: 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() diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index ddf5cc3a76cc..0979c5a175b1 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -20,7 +20,7 @@ jobs: #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"} container: - image: px4io/px4-dev-ros-melodic:2021-02-04 + image: px4io/px4-dev-ros-melodic:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 @@ -44,8 +44,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.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 @@ -73,6 +74,7 @@ jobs: 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() diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 0c022860f1bd..3adb3e565e33 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -11,7 +11,7 @@ jobs: airframe: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -20,13 +20,24 @@ jobs: - 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 - ls -ls * - # TODO: deploy to userguide gitbook and s3 + # 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-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -41,7 +52,7 @@ jobs: parameter: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -63,9 +74,36 @@ jobs: 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:2021-02-04 + container: px4io/px4-dev-nuttx-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -80,7 +118,7 @@ jobs: micrortps_agent: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -94,7 +132,7 @@ jobs: ROS_msgs: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -107,7 +145,7 @@ jobs: ROS2_bridge: runs-on: ubuntu-latest - container: px4io/px4-dev-base-focal:2021-02-04 + container: px4io/px4-dev-base-focal:2021-09-08 steps: - uses: actions/checkout@v1 with: @@ -116,4 +154,4 @@ jobs: - name: PX4 ROS2 bridge run: | git clone https://github.com/PX4/px4_ros_com.git - ./msg/tools/uorb_to_ros_rtps_ids.py -i msg/tools/uorb_rtps_message_ids.yaml -o px4_ros_com/templates/uorb_rtps_message_ids.yaml + ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index fdf316012d1c..ddad50475ccb 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -18,7 +18,7 @@ jobs: - name: Install Python3 run: sudo apt-get install python3 python3-setuptools python3-pip -y - name: Install tools - run: pip3 install --user mypy flake8 + run: pip3 install --user mypy types-requests flake8 - name: Check MAVSDK test scripts with mypy run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py - name: Check MAVSDK test scripts with flake8 diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 11ad4ebea9df..1f6b02353128 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -15,12 +15,13 @@ jobs: fail-fast: false matrix: config: - - {latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "RelWithDebInfo", model: "iris" } # Alaska - - {latitude: "-38.071235", longitude: "145.281220", altitude: "31", build_type: "RelWithDebInfo", model: "standard_vtol" } # Australia - - {latitude: "29.660316", longitude: "-82.316658", altitude: "30", build_type: "RelWithDebInfo", model: "tailsitter" } # Florida - - {latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "Coverage", model: "standard_vtol" } # Zurich + - {model: "iris", latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "RelWithDebInfo" } # Alaska + - {model: "standard_vtol", latitude: "-38.071235", longitude: "145.281220", altitude: "31", build_type: "AddressSanitizer" } # Australia + - {model: "tailsitter" , latitude: "29.660316", longitude: "-82.316658", altitude: "30", build_type: "RelWithDebInfo" } # Florida + - {model: "standard_vtol", latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "Coverage" } # Zurich + container: - image: px4io/px4-dev-simulation-focal:2021-02-04 + image: px4io/px4-dev-simulation-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 @@ -28,9 +29,9 @@ jobs: token: ${{ secrets.ACCESS_TOKEN }} - name: Download MAVSDK - run: wget https://github.com/mavlink/MAVSDK/releases/download/v0.36.0/mavsdk_0.36.0_ubuntu20.04_amd64.deb + run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" - name: Install MAVSDK - run: dpkg -i mavsdk_0.36.0_ubuntu20.04_amd64.deb + run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" - name: Prepare ccache timestamp id: ccache_cache_timestamp @@ -49,8 +50,9 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 5" >> ~/.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 @@ -94,7 +96,8 @@ jobs: PX4_HOME_LON: ${{matrix.config.longitude}} PX4_HOME_ALT: ${{matrix.config.altitude}} PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose + timeout-minutes: 45 - name: Look at core files if: failure() diff --git a/.gitignore b/.gitignore index 2e467da6244d..f1945d12eda2 100644 --- a/.gitignore +++ b/.gitignore @@ -105,6 +105,4 @@ src/lib/version/build_git_version.h src/modules/simulator/simulator_config.h src/systemcmds/topic_listener/listener_generated.cpp -# SITL -dataman -eeprom/ +!src/drivers/distance_sensor/broadcom/afbrs50/Lib/* diff --git a/.gitmodules b/.gitmodules index 482fd774ce58..22ef494793a7 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,11 +1,11 @@ -[submodule "mavlink/include/mavlink/v2.0"] - path = mavlink/include/mavlink/v2.0 - url = https://github.com/mavlink/c_library_v2.git +[submodule "src/modules/mavlink/mavlink"] + 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/PX4/libuavcan.git - branch = px4 + url = https://github.com/dronecan/libuavcan.git + branch = main [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://github.com/PX4/jMAVSim.git @@ -14,18 +14,6 @@ path = Tools/sitl_gazebo url = https://github.com/PX4/PX4-SITL_gazebo.git branch = master -[submodule "src/lib/matrix"] - path = src/lib/matrix - url = https://github.com/PX4/PX4-Matrix.git - branch = master -[submodule "src/lib/ecl"] - path = src/lib/ecl - url = https://github.com/PX4/PX4-ECL.git - branch = master -[submodule "boards/atlflight/cmake_hexagon"] - path = boards/atlflight/cmake_hexagon - url = https://github.com/PX4/cmake_hexagon.git - branch = px4 [submodule "src/drivers/gps/devices"] path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git @@ -33,33 +21,49 @@ [submodule "src/modules/micrortps_bridge/micro-CDR"] path = src/modules/micrortps_bridge/micro-CDR url = https://github.com/PX4/Micro-CDR.git - branch = px4 + branch = master [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx url = https://github.com/PX4/NuttX.git - branch = px4_firmware_nuttx-10.0.0+ + branch = px4_firmware_nuttx-10.1.0+ [submodule "platforms/nuttx/NuttX/apps"] path = platforms/nuttx/NuttX/apps url = https://github.com/PX4/NuttX-apps.git - branch = px4_firmware_nuttx-10.0.0+ -[submodule "platforms/qurt/dspal"] - path = platforms/qurt/dspal - url = https://github.com/ATLFlight/dspal.git + branch = px4_firmware_nuttx-10.1.0+ [submodule "Tools/flightgear_bridge"] path = Tools/flightgear_bridge url = https://github.com/PX4/PX4-FlightGear-Bridge.git [submodule "Tools/jsbsim_bridge"] path = Tools/jsbsim_bridge url = https://github.com/PX4/px4-jsbsim-bridge.git -[submodule "src/drivers/uavcan_v1/libcanard"] - path = src/drivers/uavcan_v1/libcanard - url = https://github.com/UAVCAN/libcanard.git -[submodule "src/drivers/uavcan_v1/public_regulated_data_types"] - path = src/drivers/uavcan_v1/public_regulated_data_types - url = https://github.com/UAVCAN/public_regulated_data_types.git -[submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"] - path = src/drivers/uavcannode_gps_demo/public_regulated_data_types - url = https://github.com/UAVCAN/public_regulated_data_types -[submodule "src/drivers/uavcannode_gps_demo/libcanard"] - path = src/drivers/uavcannode_gps_demo/libcanard - url = https://github.com/UAVCAN/libcanard +[submodule "src/drivers/cyphal/libcanard"] + path = src/drivers/cyphal/libcanard + url = https://github.com/opencyphal/libcanard.git +[submodule "src/drivers/cyphal/public_regulated_data_types"] + path = src/drivers/cyphal/public_regulated_data_types + url = https://github.com/opencyphal/public_regulated_data_types.git +[submodule "src/drivers/cyphal/legacy_data_types"] + path = src/drivers/cyphal/legacy_data_types + url = https://github.com/PX4/public_regulated_data_types.git + branch = legacy +[submodule "src/lib/crypto/monocypher"] + path = src/lib/crypto/monocypher + url = https://github.com/PX4/Monocypher.git + branch = px4 +[submodule "src/lib/events/libevents"] + path = src/lib/events/libevents + url = https://github.com/mavlink/libevents.git +[submodule "Tools/simulation-ignition"] + path = Tools/simulation-ignition + url = https://github.com/PX4/px4-simulation-ignition.git +[submodule "src/lib/crypto/libtomcrypt"] + path = src/lib/crypto/libtomcrypt + url = https://github.com/PX4/libtomcrypt.git + branch = px4 +[submodule "src/lib/crypto/libtommath"] + path = src/lib/crypto/libtommath + url = https://github.com/PX4/libtommath.git + branch = px4 +[submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"] + path = src/modules/microdds_client/Micro-XRCE-DDS-Client + url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git diff --git a/.vscode/.gitignore b/.vscode/.gitignore index 976b187e0f93..9c5e791626d6 100644 --- a/.vscode/.gitignore +++ b/.vscode/.gitignore @@ -9,3 +9,5 @@ launch.json ipch/ browse.vc.db* + +*.log diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index ebb9850906eb..9e983b46b5f1 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -6,6 +6,21 @@ CONFIG: buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default + px4_sitl_rtps: + short: px4_sitl_rtps + buildType: RelWithDebInfo + settings: + CONFIG: px4_sitl_rtps + px4_sitl_asan: + short: px4_sitl (AddressSanitizer) + buildType: AddressSanitizer + settings: + CONFIG: px4_sitl_default + px4_sitl_ubsan: + short: px4_sitl (UndefinedBehaviorSanitizer) + buildType: UndefinedBehaviorSanitizer + settings: + CONFIG: px4_sitl_default px4_sitl_replay: short: px4_sitl_replay buildType: RelWithDebInfo @@ -46,11 +61,26 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v5_default + px4_fmu-v5_debug: + short: px4_fmu-v5_debug + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v5_debug px4_fmu-v5x_default: short: px4_fmu-v5x buildType: MinSizeRel settings: CONFIG: px4_fmu-v5x_default + px4_fmu-v6x_default: + short: px4_fmu-v6x + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6x_default + px4_fmu-v6x_bootloader: + short: px4_fmu-v6x_bootloader + buildType: MinSizeRel + settings: + CONFIG: px4_fmu-v6x_bootloader airmind_mindpx-v2_default: short: airmind_mindpx-v2 buildType: MinSizeRel @@ -66,6 +96,46 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-flow_canbootloader + ark_can-gps_default: + short: ark_can-gps_default + buildType: MinSizeRel + settings: + CONFIG: ark_can-gps_default + ark_can-gps_canbootloader: + short: ark_can-gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_can-gps_canbootloader + ark_can-rtk-gps_default: + short: ark_can-rtk-gps_default + buildType: MinSizeRel + settings: + CONFIG: ark_can-rtk-gps_default + ark_can-rtk-gps_debug: + short: ark_can-rtk-gps_debug + buildType: MinSizeRel + settings: + CONFIG: ark_can-rtk-gps_debug + ark_can-rtk-gps_canbootloader: + short: ark_can-rtk-gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_can-rtk-gps_canbootloader + ark_cannode_default: + short: ark_cannode_default + buildType: MinSizeRel + settings: + CONFIG: ark_cannode_default + ark_cannode_canbootloader: + short: ark_cannode_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_cannode_canbootloader + atl_mantis-edu_default: + short: atl_mantis-edu + buildType: MinSizeRel + settings: + CONFIG: atl_mantis-edu_default av_x-v1_default: short: av_x-v1 buildType: MinSizeRel @@ -96,21 +166,26 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: cuav_x7pro_default - cubepilot_cubeorange_default: + cubepilot_cubeorange_test: short: cubepilot_cubeorange buildType: MinSizeRel settings: - CONFIG: cubepilot_orange_default - cubepilot_cubeyellow_default: - short: cubepilot_cubeyellow - buildType: MinSizeRel - settings: - CONFIG: cubepilot_cubeyellow_default + CONFIG: cubepilot_orange_test emlid_navio2_default: short: emlid_navio2 buildType: MinSizeRel settings: CONFIG: emlid_navio2_default + freefly_can-rtk-gps_default: + short: freefly_can-rtk-gps_default + buildType: MinSizeRel + settings: + CONFIG: freefly_can-rtk-gps_default + freefly_can-rtk-gps_canbootloader: + short: freefly_can-rtk-gps_canbootloader + buildType: MinSizeRel + settings: + CONFIG: freefly_can-rtk-gps_canbootloader holybro_can-gps-v1_canbootloader: short: holybro_can-gps-v1_canbootloader buildType: MinSizeRel @@ -126,11 +201,31 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: holybro_durandal-v1_default + matek_h743-slim_default: + short: matek_h743-slim + buildType: MinSizeRel + settings: + CONFIG: matek_h743-slim_default + matek_gnss-m9n-f4_canbootloader: + short: matek_gnss-m9n-f4_canbootloader + buildType: MiniSizeRel + settings: + CONFIG: matek_m9nf4can_canbootloader + matek_gnss-m9n-f4_default: + short: matek_gnss-m9n-f4_default + buildType: MiniSizeRel + settings: + CONFIG: matek_gnss-m9n-f4_default modalai_fc-v1_default: short: modalai_fc-v1 buildType: MinSizeRel settings: CONFIG: modalai_fc-v1_default + modalai_fc-v2_default: + short: modalai_fc-v2 + buildType: MinSizeRel + settings: + CONFIG: modalai_fc-v2_default mro_ctrl-zero-f7_default: short: mro_ctrl-zero-f7 buildType: MinSizeRel @@ -156,3 +251,8 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: nxp_fmuk66-v3_default + raspberrypi_pico_default: + short: raspberrypi_pico + buildType: MinSizeRel + settings: + CONFIG: raspberrypi_pico_default diff --git a/.vscode/settings.json b/.vscode/settings.json index f8ce7d827b5f..32f2836efe89 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,7 +14,6 @@ "C_Cpp.workspaceParsingPriority": "low", "cmake.buildBeforeRun": true, "cmake.buildDirectory": "${workspaceFolder}/build/${variant:CONFIG}", - "cmake.buildTask": true, "cmake.configureOnOpen": true, "cmake.ctest.parallelJobs": 1, "cmake.skipConfigureIfCachePresent": true, @@ -31,7 +30,6 @@ "esc" ], "debug.toolBarLocation": "docked", - "editor.acceptSuggestionOnEnter": "off", "editor.defaultFormatter": "chiehyu.vscode-astyle", "editor.dragAndDrop": false, "editor.insertSpaces": false, @@ -128,8 +126,6 @@ }, "search.showLineNumbers": true, "telemetry.enableTelemetry": false, - "terminal.integrated.copyOnSelection": true, - "terminal.integrated.rightClickBehavior": "paste", "terminal.integrated.scrollback": 5000, "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}", "workbench.editor.highlightModifiedTabs": true, @@ -137,5 +133,6 @@ "workbench.settings.enableNaturalLanguageSearch": false, "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" - } + }, + "cortex-debug.openocdPath": "${env:PICO_SDK_PATH}/../openocd/src/openocd" // Added for rp2040 } diff --git a/CMakeLists.txt b/CMakeLists.txt index 91de839b42b0..c1bc1b494e15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,7 +40,7 @@ # * Common functions should be included in px_base.cmake. # # * OS/ board specific fucntions should be include in -# px_impl_${PX4_PLATFORM}.cmake or px4_impl_${PX4_PLATFORM}_${PX4_BOARD}.cmake. +# px_impl_${PX4_PLATFORM}.cmake # # Formatting # --------------------------------------------------------------------------- @@ -99,10 +99,10 @@ # #============================================================================= -cmake_minimum_required(VERSION 3.2 FATAL_ERROR) +cmake_minimum_required(VERSION 3.9 FATAL_ERROR) -set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}") -set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}") +set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE) +set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE) list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/cmake) include(px4_parse_function_args) @@ -125,10 +125,19 @@ define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES FULL_DOCS "List of all PX4 module libraries" ) +define_property(GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES + BRIEF_DOCS "PX4 kernel side module libs" + FULL_DOCS "List of all PX4 kernel module libraries" + ) + define_property(GLOBAL PROPERTY PX4_MODULE_PATHS BRIEF_DOCS "PX4 module paths" FULL_DOCS "List of paths to all PX4 modules" ) +define_property(GLOBAL PROPERTY PX4_SRC_FILES + BRIEF_DOCS "src files from all PX4 modules & libs" + FULL_DOCS "SRC files from px4_add_{module,library}" + ) #============================================================================= # configuration @@ -138,13 +147,35 @@ set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration") include(px4_add_module) set(config_module_list) +set(config_kernel_list) + +# Find Python +# If using catkin, Python 2 is found since it points +# to the Python libs installed with the ROS distro +if (NOT CATKIN_DEVEL_PREFIX) + find_package(PythonInterp 3) + # We have a custom error message to tell users how to install python3. + if (NOT PYTHONINTERP_FOUND) + message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n" + " Ubuntu: sudo apt install python3 python3-dev python3-pip\n" + " macOS: brew install python") + endif() +else() + find_package(PythonInterp REQUIRED) +endif() + +option(PYTHON_COVERAGE "Python code coverage" OFF) +if(PYTHON_COVERAGE) + message(STATUS "python coverage enabled") + set(PYTHON_EXECUTABLE coverage run -p) +endif() include(px4_config) -include(px4_add_board) -include(${PX4_CONFIG_FILE}) +include(kconfig) message(STATUS "PX4 config: ${PX4_CONFIG}") message(STATUS "PX4 platform: ${PX4_PLATFORM}") + if(${PX4_PLATFORM} STREQUAL "posix") if(ENABLE_LOCKSTEP_SCHEDULER) add_definitions(-DENABLE_LOCKSTEP_SCHEDULER) @@ -185,6 +216,8 @@ if((CMAKE_BUILD_TYPE STREQUAL "Debug") OR (CMAKE_BUILD_TYPE STREQUAL "Coverage") set(MAX_CUSTOM_OPT_LEVEL -O0) elseif(CMAKE_BUILD_TYPE MATCHES "Sanitizer") set(MAX_CUSTOM_OPT_LEVEL -O1) +elseif(CMAKE_BUILD_TYPE MATCHES "Release") + set(MAX_CUSTOM_OPT_LEVEL -O3) else() if(px4_constrained_flash_build) set(MAX_CUSTOM_OPT_LEVEL -Os) @@ -201,6 +234,14 @@ message(STATUS "cmake build type: ${CMAKE_BUILD_TYPE}") # project(px4 CXX C ASM) +# Check if LTO option and check if toolchain supports it +if(LTO) + include(CheckIPOSupported) + check_ipo_supported() + message(AUTHOR_WARNING "LTO enabled: LTO is highly experimental and should not be used in production") + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION TRUE) +endif() + set(package-contact "px4users@googlegroups.com") set(CMAKE_CXX_STANDARD 14) @@ -277,27 +318,6 @@ if (CATKIN_DEVEL_PREFIX) endif() endif() -# Python -# If using catkin, Python 2 is found since it points -# to the Python libs installed with the ROS distro -if (NOT CATKIN_DEVEL_PREFIX) - find_package(PythonInterp 3) - # We have a custom error message to tell users how to install python3. - if (NOT PYTHONINTERP_FOUND) - message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n" - " Ubuntu: sudo apt install python3 python3-dev python3-pip\n" - " macOS: brew install python") - endif() -else() - find_package(PythonInterp REQUIRED) -endif() - -option(PYTHON_COVERAGE "Python code coverage" OFF) -if(PYTHON_COVERAGE) - message(STATUS "python coverage enabled") - set(PYTHON_EXECUTABLE coverage run -p) -endif() - #============================================================================= # get chip and chip manufacturer # @@ -400,6 +420,7 @@ endif() # subdirectories # add_library(parameters_interface INTERFACE) +add_library(kernel_parameters_interface INTERFACE) include(px4_add_library) add_subdirectory(src/lib EXCLUDE_FROM_ALL) @@ -415,9 +436,20 @@ foreach(module ${config_module_list}) add_subdirectory(src/${module}) endforeach() +# add events lib after modules and libs as it needs to know all source files (PX4_SRC_FILES) +add_subdirectory(src/lib/events EXCLUDE_FROM_ALL) +# metadata needs PX4_MODULE_CONFIG_FILES +add_subdirectory(src/lib/metadata EXCLUDE_FROM_ALL) + # must be the last module before firmware add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL) + +if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) +target_link_libraries(parameters_interface INTERFACE usr_parameters) +target_link_libraries(kernel_parameters_interface INTERFACE parameters) +else() target_link_libraries(parameters_interface INTERFACE parameters) +endif() # firmware added last to generate the builtin for included modules add_subdirectory(platforms/${PX4_PLATFORM}) @@ -434,8 +466,10 @@ endforeach() add_custom_command(OUTPUT ${uorb_graph_config} COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/uorb_graph/create.py - ${graph_module_list} + ${graph_module_list} --src-path src/lib + --merge-depends --exclude-path src/examples + --exclude-path src/lib/parameters # FIXME: enable & fix --file ${PX4_SOURCE_DIR}/Tools/uorb_graph/graph_${uorb_graph_config} WORKING_DIRECTORY ${PX4_SOURCE_DIR} COMMENT "Generating uORB graph" diff --git a/Jenkinsfile b/Jenkinsfile index 4cdc252ff6d9..22d47028791a 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -15,7 +15,7 @@ pipeline { // stage('Catkin build on ROS workspace') { // agent { // docker { - // image 'px4io/px4-dev-ros-melodic:2021-02-04' + // image 'px4io/px4-dev-ros-melodic:2021-08-18' // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' // } // } @@ -56,7 +56,7 @@ pipeline { stage('Colcon build on ROS2 workspace') { agent { docker { - image 'px4io/px4-dev-ros2-foxy:2021-02-04' + image 'px4io/px4-dev-ros2-foxy:2021-08-18' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -85,10 +85,11 @@ pipeline { stage('Airframe') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' + sh 'git fetch --all --tags' sh 'make airframe_metadata' dir('build/px4_sitl_default/docs') { archiveArtifacts(artifacts: 'airframes.md, airframes.xml') @@ -97,17 +98,18 @@ pipeline { } post { always { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' } } } stage('Parameter') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' + sh 'git fetch --all --tags' sh 'make parameters_metadata' dir('build/px4_sitl_default/docs') { archiveArtifacts(artifacts: 'parameters.md, parameters.xml, parameters.json.xz') @@ -116,17 +118,18 @@ pipeline { } post { always { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' } } } stage('Module') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' + sh 'git fetch --all --tags' sh 'make module_documentation' dir('build/px4_sitl_default/docs') { archiveArtifacts(artifacts: 'modules/*.md') @@ -135,7 +138,25 @@ pipeline { } post { always { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' + } + } + } + + stage('msg file docs') { + agent { + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } + } + steps { + sh 'mkdir -p build/msg_docs; ./msg/tools/generate_msg_docs.py -d build/msg_docs' + dir('build') { + archiveArtifacts(artifacts: 'msg_docs/*.md') + stash includes: 'msg_docs/*.md', name: 'msg_documentation' + } + } + post { + always { + sh 'make distclean; git clean -ff -x -d .' } } } @@ -143,22 +164,23 @@ pipeline { stage('uORB graphs') { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-02-04' + image 'px4io/px4-dev-nuttx-focal:2021-08-18' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } steps { sh 'export' - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' + sh 'git fetch --all --tags' sh 'make uorb_graphs' dir('Tools/uorb_graph') { - archiveArtifacts(artifacts: 'graph_px4_sitl.json') - stash includes: 'graph_px4_sitl.json', name: 'uorb_graph' + archiveArtifacts(artifacts: 'graph_*.json') + stash includes: 'graph_*.json', name: 'uorb_graph' } } post { always { - sh 'make distclean' + sh 'make distclean; git clean -ff -x -d .' } } } @@ -172,18 +194,22 @@ pipeline { stage('Userguide') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') unstash 'metadata_airframes' unstash 'metadata_parameters' unstash 'metadata_module_documentation' + unstash 'msg_documentation' + unstash 'uorb_graph' withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_user_guide.git') sh('cp airframes.md px4_user_guide/en/airframes/airframe_reference.md') sh('cp parameters.md px4_user_guide/en/advanced_config/parameter_reference.md') sh('cp -R modules/*.md px4_user_guide/en/modules/') + sh('cp -R graph_*.json px4_user_guide/.vuepress/public/en/middleware/') + sh('cp -R msg_docs/*.md px4_user_guide/en/msg_docs/') sh('cd px4_user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true') sh('cd px4_user_guide; git push origin master || true') sh('rm -rf px4_user_guide') @@ -202,7 +228,7 @@ pipeline { stage('QGroundControl') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') @@ -230,12 +256,12 @@ pipeline { stage('microRTPS agent') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') sh('git fetch --all --tags') - sh('make distclean') + sh('make distclean; git clean -ff -x -d .') sh('make px4_sitl_rtps') withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/micrortps_agent.git -b ${BRANCH_NAME}") @@ -260,11 +286,11 @@ pipeline { stage('PX4 ROS msgs') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') - sh('make distclean') + sh('make distclean; git clean -ff -x -d .') withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git") // 'master' branch @@ -289,15 +315,15 @@ pipeline { stage('PX4 ROS2 bridge') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') - sh('make distclean') + sh('make distclean; git clean -ff -x -d .') withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) { sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_ros_com.git -b ${BRANCH_NAME}") // deploy uORB RTPS ID map - sh('./msg/tools/uorb_to_ros_rtps_ids.py -i msg/tools/uorb_rtps_message_ids.yaml -o px4_ros_com/templates/uorb_rtps_message_ids.yaml') + sh('./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml') sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS ID map `date`" || true') sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true') // deploy uORB RTPS required tools @@ -332,7 +358,7 @@ pipeline { stage('S3') { agent { - docker { image 'px4io/px4-dev-base-focal:2021-02-04' } + docker { image 'px4io/px4-dev-base-focal:2021-08-18' } } steps { sh('export') @@ -370,7 +396,7 @@ pipeline { GIT_COMMITTER_NAME = "PX4BuildBot" } options { - buildDiscarder(logRotator(numToKeepStr: '10', artifactDaysToKeepStr: '20')) - timeout(time: 60, unit: 'MINUTES') + buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '30')) + timeout(time: 90, unit: 'MINUTES') } } diff --git a/Kconfig b/Kconfig new file mode 100644 index 000000000000..a6be4d1a8ce5 --- /dev/null +++ b/Kconfig @@ -0,0 +1,192 @@ +# PX4 Firmware Configuration + + +mainmenu "PX4 Firmware Configuration" + +comment "Vendor: $(VENDOR)" +comment "Model: $(MODEL)" +comment "Label: $(LABEL)" + +menu "Toolchain" + choice + prompt "Platform" + default PLATFORM_NUTTX + config PLATFORM_NUTTX + bool "nuttx" + config PLATFORM_POSIX + bool "posix" + config PLATFORM_QURT + bool "qurt" + endchoice + + config BOARD_PLATFORM + string + default "nuttx" if PLATFORM_NUTTX + default "posix" if PLATFORM_POSIX + default "qurt" if PLATFORM_QURT + + config BOARD_LOCKSTEP + bool "Force enable lockstep" + depends on PLATFORM_POSIX + help + forces lockstep behaviour, despite REPLAY env variable + + config BOARD_NOLOCKSTEP + bool "Force disable lockstep" + depends on PLATFORM_POSIX + help + forces nolockstep behaviour, despite REPLAY env variable + + config BOARD_LINUX + bool "Linux OS" + depends on PLATFORM_POSIX + help + Board Platform is running the Linux operating system + + config BOARD_TOOLCHAIN + string "Toolchain" + default "" + + config BOARD_ARCHITECTURE + string "Architecture" + default "" + + config BOARD_LTO + bool "(EXPERIMENTAL) Link Time Optimization (LTO)" + default n + help + Enables LTO flag in linker + Note: Highly EXPERIMENTAL, furthermore make sure you're using a modern compiler GCC 9 or later + + config BOARD_FULL_OPTIMIZATION + bool "Full optmization (O3)" + default n + help + Enables Cmake Release for -O3 optimization + + config BOARD_ROMFSROOT + string "ROMFSROOT" + default "px4fmu_common" + help + relative path to the ROMFS root directory + + config BOARD_IO + string "IO board name" + default "px4_io-v2_default" + depends on DRIVERS_PX4IO + help + name of IO board to be built and included in the ROMFS (requires a valid ROMFSROOT) + + config BOARD_CONSTRAINED_FLASH + bool "Contrained flash" + help + flag to enable constrained flash options (eg limit init script status text) + + config BOARD_NO_HELP + bool "No help" + help + optional condition flag to disable help text on constrained flash systems + + config BOARD_CONSTRAINED_MEMORY + bool "Contrained memory" + help + flag to enable constrained memory options (eg limit maximum number of uORB publications) + + config BOARD_EXTERNAL_METADATA + bool "External metadata" + help + flag to exclude metadata to reduce flash + + config BOARD_LINKER_PREFIX + string "linker prefix" + help + optional to prefix on the Linker script. + + config BOARD_COMPILE_DEFINITIONS + string "add custom compile definitions" + help + add custom compile defitions to this specific target +endmenu #Toolchain + +config BOARD_TESTING + bool "Testing" + select SYSTEMCMDS_TESTS + help + flag to enable automatic inclusion of PX4 testing modules + + +config BOARD_ETHERNET + bool "Ethernet" + help + flag to indicate that ethernet is enabled + +config BOARD_CRYPTO + bool "Crypto support" + help + Enable PX4 Crypto Support. Select the implementation under drivers + +config BOARD_PROTECTED + bool "Memory protection" + help + Enable memory protection via MPU/MMU + +menu "Serial ports" + + config BOARD_SERIAL_URT6 + string "URT6 tty port" + + config BOARD_SERIAL_GPS1 + string "GPS1 tty port" + + config BOARD_SERIAL_GPS2 + string "GPS2 tty port" + + config BOARD_SERIAL_GPS3 + string "GPS3 tty port" + + config BOARD_SERIAL_GPS4 + string "GPS4 tty port" + + config BOARD_SERIAL_GPS5 + string "GPS5 tty port" + + config BOARD_SERIAL_TEL1 + string "TEL1 tty port" + + config BOARD_SERIAL_TEL2 + string "TEL2 tty port" + + config BOARD_SERIAL_TEL3 + string "TEL3 tty port" + + config BOARD_SERIAL_TEL4 + string "TEL4 tty port" + + config BOARD_SERIAL_TEL5 + string "TEL5 tty port" + + config BOARD_SERIAL_RC + string "RC tty port" + + config BOARD_SERIAL_WIFI + string "WIFI tty port" + + config BOARD_SERIAL_PPB + string "PPB (Pixhawk Payload Bus) tty port" +endmenu + +menu "drivers" +source "src/drivers/Kconfig" +endmenu + +menu "modules" +source "src/modules/Kconfig" +endmenu + +menu "systemcmds" +source "src/systemcmds/Kconfig" +endmenu + +menu "examples" +source "src/examples/Kconfig" +endmenu diff --git a/LICENSE b/LICENSE index 1378cc40023a..f9bb8eb101ba 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ BSD 3-Clause License -Copyright (c) 2012 - 2021, PX4 Development Team +Copyright (c) 2012 - 2022, PX4 Development Team All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/Makefile b/Makefile index 99e8657960c3..32a6e17c0dab 100644 --- a/Makefile +++ b/Makefile @@ -63,7 +63,7 @@ all: px4_sitl_default space := $(subst ,, ) define make_list - $(shell cat .github/workflows/compile_${1}.yml | sed -E 's|[[:space:]]+(.*),|check_\1|g' | grep check_${2}) + $(shell [ -f .github/workflows/compile_${1}.yml ] && cat .github/workflows/compile_${1}.yml | sed -E 's|[[:space:]]+(.*),|check_\1|g' | grep check_${2}) endef # Parsing @@ -158,6 +158,11 @@ else CMAKE_ARGS += -DCMAKE_BUILD_TYPE=UndefinedBehaviorSanitizer endif + # Fuzz Testing + ifdef PX4_FUZZ + CMAKE_ARGS += -DCMAKE_BUILD_TYPE=FuzzTesting + endif + endif # Pick up specific Python path if set @@ -165,10 +170,16 @@ ifdef PYTHON_EXECUTABLE CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} endif +# Check if the microRTPS agent is to be built +ifdef BUILD_MICRORTPS_AGENT + CMAKE_ARGS += -DBUILD_MICRORTPS_AGENT=ON +endif + # Functions # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build + $(eval CMAKE_ARGS += -DCONFIG=$(1)) @$(eval BUILD_DIR = "$(SRC_DIR)/build/$(1)") @# check if the desired cmake configuration matches the cache then CMAKE_CACHE_CHECK stays empty @$(call cmake-cache-check) @@ -207,8 +218,8 @@ define colorecho +@echo -e '${COLOR_BLUE}${1} ${NO_COLOR}' endef -# Get a list of all config targets boards/*/*.cmake -ALL_CONFIG_TARGETS := $(shell find boards -maxdepth 3 -mindepth 3 ! -name '*common*' ! -name '*sdflight*' -name '*.cmake' -print | sed -e 's|boards\/||' | sed -e 's|\.cmake||' | sed -e 's|\/|_|g' | sort) +# Get a list of all config targets boards/*/*.px4board +ALL_CONFIG_TARGETS := $(shell find boards -maxdepth 3 -mindepth 3 -name '*.px4board' -print | sed -e 's|boards\/||' | sed -e 's|\.px4board||' | sed -e 's|\/|_|g' | sort) # ADD CONFIGS HERE # -------------------------------------------------------------------- @@ -216,20 +227,19 @@ ALL_CONFIG_TARGETS := $(shell find boards -maxdepth 3 -mindepth 3 ! -name '*comm # All targets. $(ALL_CONFIG_TARGETS): - @$(eval PX4_CONFIG = $@) - @$(eval CMAKE_ARGS += -DCONFIG=$(PX4_CONFIG)) - @$(call cmake-build,$(PX4_CONFIG)$(BUILD_DIR_SUFFIX)) + @$(call cmake-build,$@$(BUILD_DIR_SUFFIX)) # Filter for only default targets to allow omiting the "_default" postfix CONFIG_TARGETS_DEFAULT := $(patsubst %_default,%,$(filter %_default,$(ALL_CONFIG_TARGETS))) $(CONFIG_TARGETS_DEFAULT): - @$(eval PX4_CONFIG = $@_default) - @$(eval CMAKE_ARGS += -DCONFIG=$(PX4_CONFIG)) - @$(call cmake-build,$(PX4_CONFIG)$(BUILD_DIR_SUFFIX)) + @$(call cmake-build,$@_default$(BUILD_DIR_SUFFIX)) all_config_targets: $(ALL_CONFIG_TARGETS) all_default_targets: $(CONFIG_TARGETS_DEFAULT) +updateconfig: + @./Tools/kconfig/updateconfig.py + # board reorganization deprecation warnings (2018-11-22) define deprecation_warning $(warning $(1) has been deprecated and will be removed, please use $(2)!) @@ -238,16 +248,6 @@ endef # All targets with just dependencies but no recipe must either be marked as phony (or have the special @: as recipe). .PHONY: all px4_sitl_default all_config_targets all_default_targets -# Multi- config targets. -eagle_default: atlflight_eagle_default atlflight_eagle_qurt -eagle_rtps: atlflight_eagle_rtps atlflight_eagle_qurt-rtps - -excelsior_default: atlflight_excelsior_default atlflight_excelsior_qurt -excelsior_rtps: atlflight_excelsior_rtps atlflight_excelsior_qurt-rtps - -.PHONY: eagle_default eagle_rtps -.PHONY: excelsior_default excelsior_rtps - # Other targets # -------------------------------------------------------------------- @@ -274,7 +274,6 @@ misc_qgc_extra_firmware: \ check_bitcraze_crazyflie_default \ check_bitcraze_crazyflie21_default \ check_airmind_mindpx-v2_default \ - check_px4_fmu-v2_lpe \ sizes # builds with RTPS @@ -302,12 +301,38 @@ check_%: @$(MAKE) --no-print-directory $(subst check_,,$@) @echo +all_variants_%: + @echo 'Building all $(subst all_variants_,,$@) variants:' $(filter $(subst all_variants_,,$@)_%, $(ALL_CONFIG_TARGETS)) + @echo + $(foreach a,$(filter $(subst all_variants_,,$@)_%, $(ALL_CONFIG_TARGETS)), $(call cmake-build,$(a)$(BUILD_DIR_SUFFIX))) + uorb_graphs: - @./Tools/uorb_graph/create.py --src-path src --exclude-path src/examples --file Tools/uorb_graph/graph_full + @./Tools/uorb_graph/create.py --src-path src --exclude-path src/examples --exclude-path src/lib/parameters --merge-depends --file Tools/uorb_graph/graph_full + @./Tools/uorb_graph/create.py --src-path src --exclude-path src/examples --exclude-path src/lib/parameters --exclude-path src/modules/mavlink --merge-depends --file Tools/uorb_graph/graph_full_no_mavlink @$(MAKE) --no-print-directory px4_fmu-v2_default uorb_graph @$(MAKE) --no-print-directory px4_fmu-v4_default uorb_graph + @$(MAKE) --no-print-directory px4_fmu-v5_default uorb_graph @$(MAKE) --no-print-directory px4_sitl_default uorb_graph +px4io_update: px4_io-v2_default cubepilot_io-v2_default + # px4_io-v2_default + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/durandal-v1/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/pix32v5/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/mro/x21-777/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v2/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v3/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5x/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6x/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6c/extras/px4_io-v2_default.bin + # cubepilot_io-v2_default + cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin + cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin + git status + +bootloaders_update: cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader holybro_kakuteh7_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6c_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader + git status .PHONY: coverity_scan @@ -315,18 +340,21 @@ coverity_scan: px4_sitl_default # Documentation # -------------------------------------------------------------------- -.PHONY: parameters_metadata airframe_metadata module_documentation px4_metadata doxygen +.PHONY: parameters_metadata airframe_metadata module_documentation extract_events px4_metadata doxygen parameters_metadata: @$(MAKE) --no-print-directory px4_sitl_default metadata_parameters ver_gen airframe_metadata: - @$(MAKE) --no-print-directory px4_sitl_default metadata_airframes + @$(MAKE) --no-print-directory px4_sitl_default metadata_airframes ver_gen module_documentation: @$(MAKE) --no-print-directory px4_sitl_default metadata_module_documentation -px4_metadata: parameters_metadata airframe_metadata module_documentation +extract_events: + @$(MAKE) --no-print-directory px4_sitl_default metadata_extract_events ver_gen + +px4_metadata: parameters_metadata airframe_metadata module_documentation extract_events doxygen: @mkdir -p "$(SRC_DIR)"/build/doxygen @@ -353,7 +381,6 @@ format: .PHONY: rostest python_coverage tests: - $(eval CMAKE_ARGS += -DCONFIG=px4_sitl_test) $(eval CMAKE_ARGS += -DTESTFILTER=$(TESTFILTER)) $(eval ARGS += test_results) $(eval ASAN_OPTIONS += color=always:check_initialization_order=1:detect_stack_use_after_return=1) @@ -461,34 +488,39 @@ shellcheck_all: @make px4_fmu-v5_default shellcheck validate_module_configs: - @find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f -print0 | xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml + @find "$(SRC_DIR)"/src/modules "$(SRC_DIR)"/src/drivers "$(SRC_DIR)"/src/lib -name *.yaml -type f \ + -not -path "$(SRC_DIR)/src/lib/mixer_module/*" -not -path "$(SRC_DIR)/src/lib/crypto/libtommath/*" -print0 | \ + xargs -0 "$(SRC_DIR)"/Tools/validate_yaml.py --schema-file "$(SRC_DIR)"/validation/module_schema.yaml # Cleanup # -------------------------------------------------------------------- .PHONY: clean submodulesclean submodulesupdate gazeboclean distclean clean: - @rm -rf "$(SRC_DIR)"/build + @[ ! -d "$(SRC_DIR)/build" ] || find "$(SRC_DIR)/build" -mindepth 1 -maxdepth 1 -type d -exec sh -c "echo {}; cmake --build {} -- clean || rm -rf {}" \; # use generated build system to clean, wipe build directory if it fails + @git submodule foreach git clean -dX --force # some submodules generate build artifacts in source submodulesclean: @git submodule foreach --quiet --recursive git clean -ff -x -d @git submodule update --quiet --init --recursive --force || true @git submodule sync --recursive - @git submodule update --init --recursive --force + @git submodule update --init --recursive --force --jobs 4 submodulesupdate: - @git submodule update --quiet --init --recursive || true + @git submodule update --quiet --init --recursive --jobs 4 || true @git submodule sync --recursive - @git submodule update --init --recursive + @git submodule update --init --recursive --jobs 4 + @git fetch --all --tags --recurse-submodules=yes --jobs=4 gazeboclean: @rm -rf ~/.gazebo/* distclean: gazeboclean - @git submodule deinit -f . - @git clean -ff -x -d -e ".project" -e ".cproject" -e ".idea" -e ".settings" -e ".vscode" + @git submodule deinit --force $(SRC_DIR) + @rm -rf "$(SRC_DIR)/build" + @git clean --force -X "$(SRC_DIR)/msg/" "$(SRC_DIR)/platforms/" "$(SRC_DIR)/posix-configs/" "$(SRC_DIR)/ROMFS/" "$(SRC_DIR)/src/" "$(SRC_DIR)/test/" "$(SRC_DIR)/Tools/" -# Help / Error +# Help / Error / Misc # -------------------------------------------------------------------- # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error. @@ -502,7 +534,7 @@ help: @echo "Where is one of:" @$(MAKE) -pRrq -f $(lastword $(MAKEFILE_LIST)) : 2>/dev/null | \ awk -v RS= -F: '/^# File/,/^# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ - egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(posix|eagle|Makefile)' + egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(Makefile)' @echo @echo "Or, $(MAKE) []" @echo "Use '$(MAKE) list_config_targets' for a list of configuration targets." @@ -522,3 +554,18 @@ check_px4: $(call make_list,nuttx,"px4") \ check_nxp: $(call make_list,nuttx,"nxp") \ sizes + +ifneq ($(ROS2_WS_DIR),) + ROS2_WS_DIR := $(basename ${ROS2_WS_DIR}) +else + ROS2_WS_DIR := ~/colcon_ws +endif + +update_ros2_bridge: + @Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --all + +update_px4_ros_com: + @Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_ros_com + +update_px4_msgs: + @Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs diff --git a/README.md b/README.md index 1675604d2b3b..0425d99e748f 100644 --- a/README.md +++ b/README.md @@ -4,11 +4,11 @@ [![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) -[![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io) +[![Slack](/.github/slack.svg)](https://join.slack.com/t/px4/shared_invite/zt-si4xo5qs-R4baYFmMjlrT4rQK5yUnaA) This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones. -PX4 is highly portable, OS-independent and supports Linux, NuttX and QuRT out of the box. +PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box. * Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE)) * [Supported airframes](https://docs.px4.io/master/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)): @@ -17,7 +17,7 @@ PX4 is highly portable, OS-independent and supports Linux, NuttX and QuRT out of * [VTOL](https://docs.px4.io/master/en/frames_vtol/) * [Autogyro](https://docs.px4.io/master/en/frames_autogyro/) * [Rover](https://docs.px4.io/master/en/frames_rover/) - * many more experimental types (Blimps, Boats, Submarines, Autogyros, etc) + * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc) * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases) @@ -110,7 +110,7 @@ This repository contains code supporting Pixhawk standard boards (best supported * [Hex Cube Yellow](https://docs.px4.io/master/en/flight_controller/cubepilot_cube_yellow.html) * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf) * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf) - * [Bitcraze Crazyflie 2.0](https://docs.px4.io/master/en/flight_controller/crazyflie2.html) + * [Bitcraze Crazyflie 2.0](https://docs.px4.io/master/en/complete_vehicles/crazyflie2.html) * [Omnibus F4 SD](https://docs.px4.io/master/en/flight_controller/omnibus_f4_sd.html) * [Holybro Kakute F7](https://docs.px4.io/master/en/flight_controller/kakutef7.html) * [Raspberry PI with Navio 2](https://docs.px4.io/master/en/flight_controller/raspberry_pi_navio2.html) diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index 3b1c1fd49606..2b2a75f88864 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -115,10 +115,12 @@ add_custom_command( set(romfs_extract_stamp ${CMAKE_CURRENT_BINARY_DIR}/romfs_extract.stamp) add_custom_command( OUTPUT ${romfs_extract_stamp} + COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/* COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file} COMMAND ${CMAKE_COMMAND} -E touch ${romfs_extract_stamp} WORKING_DIRECTORY ${romfs_gen_root_dir} DEPENDS ${romfs_tar_file} + VERBATIM ) add_custom_command( @@ -141,37 +143,13 @@ add_custom_command( COMMENT "ROMFS: copying, generating airframes" ) -# copy extras into ROMFS -set(extras_dependencies) - -# copy px4io binary if configured -if(config_io_board) - list(APPEND extras_dependencies - copy_px4io_bin - ${fw_io_bin} - ) - file(RELATIVE_PATH iofw_file_relative ${PX4_SOURCE_DIR} ${fw_io_bin}) - message(STATUS "ROMFS: Adding ${iofw_file_relative} -> /etc/extras/${config_io_board}.bin") -endif() - - -# board bootloader (built or included) -if(TARGET copy_bootloader_bin) - - if(board_bootloader_firmware) - file(RELATIVE_PATH bl_file_relative ${PX4_SOURCE_DIR} ${board_bootloader_firmware}) - message(STATUS "ROMFS: Adding ${bl_file_relative} -> /etc/extras/bootloader.bin") - else() - file(RELATIVE_PATH bl_file_relative ${PX4_SOURCE_DIR} ${bootloader_bin}) - message(STATUS "ROMFS: Adding ${bl_file_relative} -> /etc/extras/bootloader.bin") - endif() - - list(APPEND extras_dependencies - copy_bootloader_bin - ${bootloader_bin} - ) +if(EXISTS ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin) + set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin") + configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY) endif() +# copy extras into ROMFS +set(extras_dependencies) # optional board architecture defaults set(board_arch_rc_file "rc.board_arch_defaults") @@ -197,6 +175,7 @@ if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/init/${CONFIG_ARCH_CHIP}/ endif() +# board custom init files set(OPTIONAL_BOARD_RC) list(APPEND OPTIONAL_BOARD_RC rc.board_defaults @@ -231,21 +210,59 @@ foreach(board_rc_file ${OPTIONAL_BOARD_RC}) endforeach() +# board extras +set(OPTIONAL_BOARD_EXTRAS) +file(GLOB OPTIONAL_BOARD_EXTRAS ${PX4_BOARD_DIR}/extras/*) + +foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS}) + + if(EXISTS "${board_extra_file}") + file(RELATIVE_PATH extra_file_base_name ${PX4_BOARD_DIR}/extras/ ${board_extra_file}) + file(RELATIVE_PATH extra_file_relative_source ${PX4_SOURCE_DIR} ${board_extra_file}) + message(STATUS "ROMFS: Adding ${extra_file_relative_source} -> /etc/extras/${extra_file_base_name}") + + add_custom_command( + OUTPUT + ${romfs_gen_root_dir}/extras/${extra_file_base_name} + ${extra_file_base_name}.stamp + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${PX4_BOARD_DIR}/extras/${extra_file_base_name} ${romfs_gen_root_dir}/extras/${extra_file_base_name} + COMMAND ${CMAKE_COMMAND} -E touch ${extra_file_base_name}.stamp + DEPENDS + ${board_extra_file} + romfs_copy.stamp + COMMENT "ROMFS: copying ${board_extra_file}" + ) + + list(APPEND extras_dependencies + ${extra_file_base_name}.stamp + ) + endif() + +endforeach() + + + if(config_uavcan_peripheral_firmware) include(ExternalProject) foreach(uavcan_peripheral_config ${config_uavcan_peripheral_firmware}) - # include the px4io binary in ROMFS + # include the UAVCAN peripheral binaries in ROMFS message(STATUS "ROMFS: Adding UAVCAN peripheral ${uavcan_peripheral_config} -> /etc/uavcan/fw/") + + # ExternalProject_Add() with GIT_SUBMODULES "" initializes no submodules. + cmake_policy(SET CMP0097 NEW) + + include(ExternalProject) + ExternalProject_Add(build_${uavcan_peripheral_config} - SOURCE_DIR ${CMAKE_SOURCE_DIR} - DOWNLOAD_COMMAND "" - UPDATE_COMMAND "" + GIT_REPOSITORY ${CMAKE_SOURCE_DIR} + GIT_TAG ${PX4_GIT_TAG} + GIT_SUBMODULES "" + DOWNLOAD_NO_PROGRESS true CMAKE_ARGS -DCONFIG=${uavcan_peripheral_config} INSTALL_COMMAND "" USES_TERMINAL_BUILD true - DEPENDS git_nuttx git_nuttx_apps BUILD_ALWAYS 1 ) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index fea50019df39..519f9db88995 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -15,15 +15,6 @@ set +e # #------------------------------------------------------------------------------ set R / -# -# Mount the procfs. -# -mount -t procfs /proc - -# -# Start CDC/ACM serial driver. -# -sercon # # Print full system version. @@ -95,10 +86,28 @@ unset BOARD_RC_SENSORS . ${R}etc/init.d/rc.serial # Check for flow sensor -if param compare SENS_EN_PX4FLOW 1 +if param compare -s SENS_EN_PX4FLOW 1 +then + px4flow start -X & +fi + +if param compare -s IMU_GYRO_CAL_EN 1 then - px4flow start -X + gyro_calibration start fi + +if param compare -s MBE_ENABLE 1 +then + # conservative mag bias estimation + param set-default MBE_LEARN_GAIN 5 + param set-default IMU_GYRO_CUTOFF 20 + mag_bias_estimator start +fi + +param set-default SENS_MAG_RATE 100 + +sensors start + uavcannode start unset R diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index b64d7d8ef565..419ed0ed1859 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -34,6 +34,10 @@ add_subdirectory(airframes) px4_add_romfs_files( + px4-rc.mavlink + px4-rc.params + px4-rc.rtps + px4-rc.simulator rc.replay rcS ) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc index 042c2384c34a..7871be23293c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc @@ -8,45 +8,30 @@ # . ${R}etc/init.d/rc.mc_defaults -. ${R}etc/init.d/rc.ctrlalloc -if [ $AUTOCNF = yes ] -then - param set MPC_USE_HTE 0 +param set-default SYS_CTRL_ALLOC 1 - param set VM_MASS 1.5 - param set VM_INERTIA_XX 0.03 - param set VM_INERTIA_YY 0.03 - param set VM_INERTIA_ZZ 0.05 - param set CA_AIRFRAME 0 - param set CA_METHOD 1 - param set CA_ACT0_MIN 0.0 - param set CA_ACT1_MIN 0.0 - param set CA_ACT2_MIN 0.0 - param set CA_ACT3_MIN 0.0 - param set CA_ACT0_MAX 1.0 - param set CA_ACT1_MAX 1.0 - param set CA_ACT2_MAX 1.0 - param set CA_ACT3_MAX 1.0 +param set-default CA_AIRFRAME 0 - param set CA_MC_R0_PX 0.1515 - param set CA_MC_R0_PY 0.245 - param set CA_MC_R0_CT 6.5 - param set CA_MC_R0_KM 0.05 - param set CA_MC_R1_PX -0.1515 - param set CA_MC_R1_PY -0.1875 - param set CA_MC_R1_CT 6.5 - param set CA_MC_R1_KM 0.05 - param set CA_MC_R2_PX 0.1515 - param set CA_MC_R2_PY -0.245 - param set CA_MC_R2_CT 6.5 - param set CA_MC_R2_KM -0.05 - param set CA_MC_R3_PX -0.1515 - param set CA_MC_R3_PY 0.1875 - param set CA_MC_R3_CT 6.5 - param set CA_MC_R3_KM -0.05 +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 -fi +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 -set MIXER direct +set MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10018_iris_foggy_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/10018_iris_foggy_lidar index 28d7c4312714..935b1be4cb0b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10018_iris_foggy_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10018_iris_foggy_lidar @@ -7,8 +7,5 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - param set EKF2_RNG_AID 1 - param set EKF2_RNG_A_HMAX 10 -fi +param set-default EKF2_RNG_A_HMAX 10 + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_omnicopter new file mode 100644 index 000000000000..4e3491dd7158 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_omnicopter @@ -0,0 +1,101 @@ +#!/bin/sh +# +# @name 6DoF Omnicopter SITL +# +# @type Quadrotor Wide +# +# @maintainer Jaeyoung Lim +# + +. ${R}etc/init.d/rc.mc_defaults + +param set-default SYS_CTRL_ALLOC 1 + + +param set-default CA_AIRFRAME 0 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +param set-default CA_ROTOR0_PX 0.14435 +param set-default CA_ROTOR0_PY -0.14435 +param set-default CA_ROTOR0_PZ -0.14435 +param set-default CA_ROTOR0_KM 0.05 # CCW +param set-default CA_ROTOR0_AX -0.788675 +param set-default CA_ROTOR0_AY -0.211325 +param set-default CA_ROTOR0_AZ -0.57735 + +param set-default CA_ROTOR1_PX -0.14435 +param set-default CA_ROTOR1_PY -0.14435 +param set-default CA_ROTOR1_PZ -0.14435 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_AX 0.211325 +param set-default CA_ROTOR1_AY -0.788675 +param set-default CA_ROTOR1_AZ 0.57735 + +param set-default CA_ROTOR2_PX 0.14435 +param set-default CA_ROTOR2_PY 0.14435 +param set-default CA_ROTOR2_PZ -0.14435 +param set-default CA_ROTOR2_KM 0.05 +param set-default CA_ROTOR2_AX -0.211325 +param set-default CA_ROTOR2_AY 0.788675 +param set-default CA_ROTOR2_AZ 0.57735 + +param set-default CA_ROTOR3_PX -0.14435 +param set-default CA_ROTOR3_PY 0.14435 +param set-default CA_ROTOR3_PZ -0.14435 +param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR3_AX 0.788675 +param set-default CA_ROTOR3_AY 0.211325 +param set-default CA_ROTOR3_AZ -0.57735 + +param set-default CA_ROTOR4_PX 0.14435 +param set-default CA_ROTOR4_PY -0.14435 +param set-default CA_ROTOR4_PZ 0.14435 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 0.788675 +param set-default CA_ROTOR4_AY 0.211325 +param set-default CA_ROTOR4_AZ -0.57735 + +param set-default CA_ROTOR5_PX -0.14435 +param set-default CA_ROTOR5_PY -0.14435 +param set-default CA_ROTOR5_PZ 0.14435 +param set-default CA_ROTOR5_KM 0.05 +param set-default CA_ROTOR5_AX -0.211325 +param set-default CA_ROTOR5_AY 0.788675 +param set-default CA_ROTOR5_AZ 0.57735 + +param set-default CA_ROTOR6_PX 0.14435 +param set-default CA_ROTOR6_PY 0.14435 +param set-default CA_ROTOR6_PZ 0.14435 +param set-default CA_ROTOR6_KM 0.05 +param set-default CA_ROTOR6_AX 0.211325 +param set-default CA_ROTOR6_AY -0.788675 +param set-default CA_ROTOR6_AZ 0.57735 + +param set-default CA_ROTOR7_PX -0.14435 +param set-default CA_ROTOR7_PY 0.14435 +param set-default CA_ROTOR7_PZ 0.14435 +param set-default CA_ROTOR7_KM 0.05 +param set-default CA_ROTOR7_AX -0.788675 +param set-default CA_ROTOR7_AY -0.211325 +param set-default CA_ROTOR7_AZ -0.57735 + +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 + +# disable MC desaturation which improves attitude tracking +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 MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a index 80f44d814b6d..bbe6f6b6c76a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a @@ -8,15 +8,12 @@ . ${R}etc/init.d/rc.mc_defaults -if [ $AUTOCNF = yes ] -then - # EKF2: Multi GPS blending (as the model has 2 GPS's) - param set EKF2_GPS_MASK 7 # Uses speed, hpos and vpos accuracy - param set TRIG_INTERFACE 3 - param set TRIG_MODE 4 - param set MNT_MODE_IN 4 - param set MNT_MODE_OUT 2 - param set MNT_DO_STAB 2 -fi +# EKF2: Multi GPS blending (as the model has 2 GPS's) +param set-default SENS_GPS_MASK 7 +param set-default TRIG_INTERFACE 3 +param set-default TRIG_MODE 4 +param set-default MNT_MODE_IN 4 +param set-default MNT_MODE_OUT 2 +param set-default MNT_DO_STAB 2 set MIXER quad_x diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx new file mode 100644 index 000000000000..1586ea781474 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name QuadrotorX SITL for SIH +# +# @type Quadrotor +# +# @maintainer Romain Chiappinelli +# + +. ${R}etc/init.d/rc.mc_defaults + +set MIXER quad_x + +# 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 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set SIH_VEHICLE_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane new file mode 100644 index 000000000000..e5e33fef9f63 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane @@ -0,0 +1,33 @@ +#!/bin/sh +# +# @name Plane SITL for SIH +# +# @type Plane +# +# @maintainer Romain Chiappinelli + +. ${R}etc/init.d/rc.fw_defaults + +set MIXER AERT + +# 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 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set-default BAT_N_CELLS 3 + +param set SIH_T_MAX 6.0 +param set SIH_MASS 0.3 +param set SIH_IXX 0.00402 +param set SIH_IYY 0.0144 +param set SIH_IZZ 0.0177 +param set SIH_IXZ 0.00046 +param set SIH_KDV 0.2 + +param set SIH_VEHICLE_TYPE 1 # sih as fixed wing +param set RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert new file mode 100644 index 000000000000..7d0d056caa63 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert @@ -0,0 +1,45 @@ +#!/bin/sh +# +# @name SIH Tailsitter Duo +# +# @type VTOL +# +# @maintainer Romain Chiappinelli + +. ${R}etc/init.d/rc.vtol_defaults + +param set-default VT_ELEV_MC_LOCK 0 +param set-default VT_TYPE 0 +param set-default VT_FW_DIFTHR_EN 1 +param set-default VT_FW_DIFTHR_SC 0.3 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default MAV_TYPE 19 +set MAV_TYPE 19 +set MIXER vtol_tailsitter_duo_sat + +# 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 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set-default BAT_N_CELLS 3 + +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.0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as tailsitter +param set SIH_VEHICLE_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow index eabbebbf2bf2..6003d04d16d8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow @@ -7,16 +7,18 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # EKF2 - param set EKF2_AID_MASK 2 - param set EKF2_EVP_NOISE 0.05 - param set EKF2_EVA_NOISE 0.05 +# EKF2 +param set-default EKF2_AID_MASK 2 +param set-default EKF2_EVP_NOISE 0.05 +param set-default EKF2_EVA_NOISE 0.05 - # LPE: Flow-only mode - param set LPE_FUSION 242 - param set LPE_FAKE_ORIGIN 1 +# LPE: Flow-only mode +param set-default LPE_FUSION 242 +param set-default LPE_FAKE_ORIGIN 1 - param set MPC_ALT_MODE 2 -fi +param set-default MPC_ALT_MODE 2 + +param set-default SENS_FLOW_ROT 6 +param set-default SENS_FLOW_MINHGT 0.7 +param set-default SENS_FLOW_MAXHGT 3.0 +param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_iris_irlock b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_iris_irlock index c0f9e6dfef78..7850d8b1586c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1011_iris_irlock +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1011_iris_irlock @@ -7,12 +7,10 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # enable fusion of landing target velocity - param set LTEST_MODE 1 - param set PLD_HACC_RAD 0.1 -fi +# enable fusion of landing target velocity +param set-default LTEST_MODE 1 +param set-default PLD_HACC_RAD 0.1 +param set-default RTL_PLD_MD 2 # Start up Landing Target Estimator module landing_target_estimator start diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1012_iris_rplidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1012_iris_rplidar index 0e6c5096b022..4a67b4c123d4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1012_iris_rplidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1012_iris_rplidar @@ -7,7 +7,5 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - param set LPE_FUSION 242 -fi +param set-default LPE_FUSION 242 + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision index abb53b425490..d0b8a5d07a98 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision @@ -7,15 +7,13 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # EKF2: Vision position and heading - param set EKF2_AID_MASK 24 - param set EKF2_EV_DELAY 5 +# EKF2: Vision position and heading +param set-default EKF2_AID_MASK 24 +param set-default EKF2_EV_DELAY 5 - # LPE: Vision + baro - param set LPE_FUSION 132 +# LPE: Vision + baro +param set-default LPE_FUSION 132 + +# AEQ: External heading set to use vision input +param set-default ATT_EXT_HDG_M 1 - # AEQ: External heading set to use vision input - param set ATT_EXT_HDG_M 1 -fi diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_solo b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_solo index a4dd4d731e27..45ef72836e9c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_solo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_solo @@ -7,10 +7,7 @@ . ${R}etc/init.d/rc.mc_defaults -if [ $AUTOCNF = yes ] -then - param set MC_PITCHRATE_P 0.1 - param set MC_ROLLRATE_P 0.1 -fi +param set-default MC_PITCHRATE_P 0.1 +param set-default MC_ROLLRATE_P 0.05 set MIXER quad_x diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid index ae56b441371e..62b16e50c85b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid @@ -7,8 +7,6 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - param set COM_OBS_AVOID 1 - param set MPC_XY_CRUISE 5.0 -fi +param set-default COM_OBS_AVOID 1 +param set-default MPC_XY_CRUISE 5.0 + diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_iris_rtps b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_iris_rtps deleted file mode 100644 index bc39d78a9f45..000000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_iris_rtps +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/sh -# -# @name 3DR Iris Quadrotor SITL (RTPS) -# -# @type Quadrotor Wide -# - -. ${R}etc/init.d-posix/airframes/10016_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup index bc3d997a5c50..d2c0dcf4cba7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup @@ -7,15 +7,11 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # EKF2 - param set EKF2_AID_MASK 2 - param set SENS_FLOW_ROT 0 +# EKF2 +param set-default EKF2_AID_MASK 2 - # LPE: Flow-only mode - param set LPE_FUSION 242 - param set LPE_FAKE_ORIGIN 1 +# LPE: Flow-only mode +param set-default LPE_FUSION 242 +param set-default LPE_FAKE_ORIGIN 1 - param set MPC_ALT_MODE 2 -fi +param set-default MPC_ALT_MODE 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1018_iris_vision_velocity b/ROMFS/px4fmu_common/init.d-posix/airframes/1018_iris_vision_velocity index defabcd28b44..f96412d622b0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1018_iris_vision_velocity +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1018_iris_vision_velocity @@ -7,9 +7,6 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # EKF2: Vision velocity and heading - param set EKF2_AID_MASK 272 - param set EKF2_EV_DELAY 5 -fi +# EKF2: Vision velocity and heading +param set-default EKF2_AID_MASK 272 +param set-default EKF2_EV_DELAY 5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1019_iris_dual_gps b/ROMFS/px4fmu_common/init.d-posix/airframes/1019_iris_dual_gps index 8e4c86fec8b8..6f102d93529f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1019_iris_dual_gps +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1019_iris_dual_gps @@ -7,8 +7,5 @@ . ${R}etc/init.d-posix/airframes/10016_iris -if [ $AUTOCNF = yes ] -then - # EKF2: Multi GPS blending - param set EKF2_GPS_MASK 7 # Uses speed, hpos and vpos accuracy -fi +# EKF2: Multi GPS blending +param set-default SENS_GPS_MASK 7 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1020_uuv_generic b/ROMFS/px4fmu_common/init.d-posix/airframes/1020_uuv_generic index 6f8de16cae1c..77580fb1a64f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1020_uuv_generic +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1020_uuv_generic @@ -5,19 +5,8 @@ . ${R}etc/init.d/rc.uuv_defaults -if [ $AUTOCNF = yes ] -then - #Set data link loss failsafe mode (0: disabled) - param set NAV_DLL_ACT 0 - - # disable circuit breaker for airspeed sensor - param set CBRK_AIRSPD_CHK 162128 - - #param set CBRK_GPSFAIL 240024 -fi - -set MAV_TYPE 12 -param set MAV_TYPE ${MAV_TYPE} +# disable circuit breaker for airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_uuv_hippocampus b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_uuv_hippocampus index 058a1024507b..d4bf9a22fcc1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_uuv_hippocampus +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_uuv_hippocampus @@ -5,19 +5,8 @@ . ${R}etc/init.d/rc.uuv_defaults -if [ $AUTOCNF = yes ] -then - #Set data link loss failsafe mode (0: disabled) - param set NAV_DLL_ACT 0 - - # disable circuit breaker for airspeed sensor - param set CBRK_AIRSPD_CHK 162128 - - #param set CBRK_GPSFAIL 240024 -fi - -set MAV_TYPE 12 -param set MAV_TYPE ${MAV_TYPE} +# disable circuit breaker for airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy index 34acfaad46d2..d22400846469 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_uuv_bluerov2_heavy @@ -5,16 +5,65 @@ . ${R}etc/init.d/rc.uuv_defaults -if [ $AUTOCNF = yes ] -then - #Set data link loss failsafe mode (0: disabled) - param set NAV_DLL_ACT 0 +# disable circuit breaker for airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 - # disable circuit breaker for airspeed sensor - param set CBRK_AIRSPD_CHK 162128 +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 7 -fi +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 +param set-default CA_ROTOR0_AX 1.0000 +param set-default CA_ROTOR0_AY -1.0000 +param set-default CA_ROTOR0_AZ 0.0000 +param set-default CA_ROTOR0_KM 0.0000 +param set-default CA_ROTOR0_PX 0.5000 +param set-default CA_ROTOR0_PY 0.3000 +param set-default CA_ROTOR0_PZ 0.2000 +param set-default CA_ROTOR1_AX 1.0000 +param set-default CA_ROTOR1_AY 1.0000 +param set-default CA_ROTOR1_AZ 0.0000 +param set-default CA_ROTOR1_KM 0.0000 +param set-default CA_ROTOR1_PX 0.5000 +param set-default CA_ROTOR1_PY -0.3000 +param set-default CA_ROTOR1_PZ 0.2000 +param set-default CA_ROTOR2_AX 1.0000 +param set-default CA_ROTOR2_AY 1.0000 +param set-default CA_ROTOR2_AZ 0.0000 +param set-default CA_ROTOR2_KM 0.0000 +param set-default CA_ROTOR2_PX -0.5000 +param set-default CA_ROTOR2_PY 0.3000 +param set-default CA_ROTOR2_PZ 0.2000 +param set-default CA_ROTOR3_AX 1.0000 +param set-default CA_ROTOR3_AY -1.0000 +param set-default CA_ROTOR3_AZ 0.0000 +param set-default CA_ROTOR3_KM 0.0000 +param set-default CA_ROTOR3_PX -0.5000 +param set-default CA_ROTOR3_PY -0.3000 +param set-default CA_ROTOR3_PZ 0.2000 +param set-default CA_ROTOR4_AZ -1.0000 +param set-default CA_ROTOR4_KM 0.0000 +param set-default CA_ROTOR4_PX 0.5000 +param set-default CA_ROTOR4_PY 0.5000 +param set-default CA_ROTOR5_AZ 1.0000 +param set-default CA_ROTOR5_KM 0.0000 +param set-default CA_ROTOR5_PX 0.5000 +param set-default CA_ROTOR5_PY -0.5000 +param set-default CA_ROTOR6_AZ 1.0000 +param set-default CA_ROTOR6_KM 0.0000 +param set-default CA_ROTOR6_PX -0.5000 +param set-default CA_ROTOR6_PY 0.5000 +param set-default CA_ROTOR7_KM 0.0000 +param set-default CA_ROTOR7_PX -0.5000 +param set-default CA_ROTOR7_PY -0.5000 -set PWM_OUT 12345678 -set MIXER_FILE etc/mixers-sitl/vectored6dof_sitl.main.mix -set MIXER custom +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 + +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane index 9f1bcec6f227..b84da715cf5d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane @@ -5,54 +5,72 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - param set EKF2_MAG_ACCLIM 0 - param set EKF2_MAG_YAWLIM 0 +param set-default EKF2_MAG_ACCLIM 0 +param set-default EKF2_MAG_YAWLIM 0 - param set FW_LND_AIRSPD_SC 1 - param set FW_LND_ANG 8 - param set FW_THR_LND_MAX 0 +param set-default FW_LND_AIRSPD_SC 1 +param set-default FW_LND_ANG 8 +param set-default FW_THR_LND_MAX 0 - param set FW_L1_PERIOD 12 +param set-default FW_L1_PERIOD 12 - param set FW_MAN_P_MAX 30 +param set-default FW_MAN_P_MAX 30 - param set FW_PR_I 0.4 - param set FW_PR_P 0.9 - param set FW_PR_FF 0.2 - param set FW_PSP_OFF 2 - param set FW_P_LIM_MAX 32 - param set FW_P_LIM_MIN -15 +param set-default FW_PR_P 0.9 +param set-default FW_PR_FF 0.2 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MAX 32 +param set-default FW_P_LIM_MIN -15 - param set FW_RR_FF 0.1 - param set FW_RR_P 0.3 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_P 0.3 - param set FW_THR_MAX 0.6 - param set FW_THR_MIN 0.05 - param set FW_THR_CRUISE 0.25 +param set-default FW_SPOILERS_LND 0.4 - param set FW_T_ALT_TC 2 - param set FW_T_CLMB_MAX 8 - param set FW_T_HRATE_FF 0.5 - param set FW_T_SINK_MAX 2.7 - param set FW_T_SINK_MIN 2.2 - param set FW_T_TAS_TC 2 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_THR_CRUISE 0.25 - param set FW_W_EN 1 +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 MIS_LTRMIN_ALT 30 - param set MIS_TAKEOFF_ALT 30 +param set-default FW_W_EN 1 - param set NAV_ACC_RAD 15 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 50 +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 30 - param set RWTO_TKOFF 1 +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +param set-default RWTO_TKOFF 1 + +#param set-default SYS_CTRL_ALLOC 1 +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 6 +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.0 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TYPE 4 +param set-default CA_SV_CS4_TYPE 9 +param set-default CA_SV_CS5_TYPE 10 +param set-default PWM_MAIN_FUNC3 204 +param set-default PWM_MAIN_FUNC4 205 +param set-default PWM_MAIN_FUNC5 101 +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_FUNC8 203 +param set-default PWM_MAIN_FUNC9 206 +param set-default PWM_MAIN_REV 256 -fi set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_plane_cam b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_plane_cam index 1d226793d4ae..b825ad3c9069 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_plane_cam +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_plane_cam @@ -5,10 +5,8 @@ . ${R}etc/init.d-posix/airframes/1030_plane -if [ $AUTOCNF = yes ] -then - # Camera trigger interface is MAVLink - param set TRIG_INTERFACE 3 - # Distance trigger mode enabled - param set TRIG_MODE 4 -fi +# Camera trigger interface is MAVLink +param set-default TRIG_INTERFACE 3 + +# Distance trigger mode enabled +param set-default TRIG_MODE 4 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_plane_catapult b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_plane_catapult index 17674d597b9e..f64918188a4b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_plane_catapult +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_plane_catapult @@ -5,8 +5,5 @@ . ${R}etc/init.d-posix/airframes/1030_plane -if [ $AUTOCNF = yes ] -then - param set RWTO_TKOFF 0 +param set-default RWTO_TKOFF 0 -fi diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_plane_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_plane_lidar index e197c2d91f35..18f93ee04e3f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_plane_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_plane_lidar @@ -5,8 +5,5 @@ . ${R}etc/init.d-posix/airframes/1030_plane -if [ $AUTOCNF = yes ] -then - param set FW_LND_USETER 1 +param set-default FW_LND_USETER 1 -fi diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal index 92c5f99784d7..df50472545c1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal @@ -5,54 +5,44 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - #param set EKF2_MAG_ACCLIM 0 - #param set EKF2_MAG_YAWLIM 0 - - param set FW_LND_AIRSPD_SC 1.1 - param set FW_LND_ANG 5 - param set FW_THR_LND_MAX 0 - param set FW_LND_HHDIST 30 - param set FW_LND_FL_PMIN 9.5 - param set FW_LND_FL_PMAX 20 - param set FW_LND_FLALT 5 - param set FW_LND_TLALT 15 - - param set FW_L1_PERIOD 25 - - param set FW_P_TC 0.4 - param set FW_PR_FF 0.40 - param set FW_PR_I 0.05 - param set FW_PR_P 0.05 - - param set FW_R_TC 0.45 - param set FW_RR_FF 0.40 - param set FW_RR_I 0.132 - param set FW_RR_P 0.085 - - param set FW_W_EN 1 - - param set MIS_LTRMIN_ALT 30 - param set MIS_TAKEOFF_ALT 20 - param set MIS_DIST_1WP 2500 - param set MIS_DIST_WPS 10000 - - param set NAV_ACC_RAD 15 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 50 - - param set RWTO_TKOFF 1 - #param set FW_THR_SLEW_MAX 0.3 # fix takeoff failure for JSBsim in autonomous mission mode. - param set RWTO_MAX_PITCH 20 - param set RWTO_MAX_ROLL 10 - #param set RWTO_MAX_THR 0.6 # fix takeoff failure for JSBsim in autonomous mission mode. - param set RWTO_PSP 8 - param set RWTO_AIRSPD_SCL 1.8 - -fi +param set-default FW_LND_AIRSPD_SC 1.1 +param set-default FW_LND_ANG 5 +param set-default FW_THR_LND_MAX 0 +param set-default FW_LND_HHDIST 30 +param set-default FW_LND_FL_PMIN 9.5 +param set-default FW_LND_FL_PMAX 20 +param set-default FW_LND_FLALT 5 +param set-default FW_LND_TLALT 15 + +param set-default FW_L1_PERIOD 25 + +param set-default FW_PR_FF 0.40 +param set-default FW_PR_I 0.05 +param set-default FW_PR_P 0.05 + +param set-default FW_R_TC 0.45 +param set-default FW_RR_FF 0.40 +param set-default FW_RR_I 0.132 +param set-default FW_RR_P 0.085 + +param set-default FW_W_EN 1 + +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 20 +param set-default MIS_DIST_1WP 2500 +param set-default MIS_DIST_WPS 10000 + +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +param set-default RWTO_TKOFF 1 + +param set-default RWTO_MAX_PITCH 20 +param set-default RWTO_MAX_ROLL 10 + +param set-default RWTO_PSP 8 +param set-default RWTO_AIRSPD_SCL 1.8 + set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric index 92c5f99784d7..0206ebea2fb6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric @@ -5,54 +5,43 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - #param set EKF2_MAG_ACCLIM 0 - #param set EKF2_MAG_YAWLIM 0 - - param set FW_LND_AIRSPD_SC 1.1 - param set FW_LND_ANG 5 - param set FW_THR_LND_MAX 0 - param set FW_LND_HHDIST 30 - param set FW_LND_FL_PMIN 9.5 - param set FW_LND_FL_PMAX 20 - param set FW_LND_FLALT 5 - param set FW_LND_TLALT 15 - - param set FW_L1_PERIOD 25 - - param set FW_P_TC 0.4 - param set FW_PR_FF 0.40 - param set FW_PR_I 0.05 - param set FW_PR_P 0.05 - - param set FW_R_TC 0.45 - param set FW_RR_FF 0.40 - param set FW_RR_I 0.132 - param set FW_RR_P 0.085 - - param set FW_W_EN 1 - - param set MIS_LTRMIN_ALT 30 - param set MIS_TAKEOFF_ALT 20 - param set MIS_DIST_1WP 2500 - param set MIS_DIST_WPS 10000 - - param set NAV_ACC_RAD 15 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 50 - - param set RWTO_TKOFF 1 - #param set FW_THR_SLEW_MAX 0.3 # fix takeoff failure for JSBsim in autonomous mission mode. - param set RWTO_MAX_PITCH 20 - param set RWTO_MAX_ROLL 10 - #param set RWTO_MAX_THR 0.6 # fix takeoff failure for JSBsim in autonomous mission mode. - param set RWTO_PSP 8 - param set RWTO_AIRSPD_SCL 1.8 - -fi +param set-default FW_LND_AIRSPD_SC 1.1 +param set-default FW_LND_ANG 5 +param set-default FW_THR_LND_MAX 0 +param set-default FW_LND_HHDIST 30 +param set-default FW_LND_FL_PMIN 9.5 +param set-default FW_LND_FL_PMAX 20 +param set-default FW_LND_FLALT 5 +param set-default FW_LND_TLALT 15 + +param set-default FW_L1_PERIOD 25 + +param set-default FW_PR_FF 0.40 +param set-default FW_PR_I 0.05 +param set-default FW_PR_P 0.05 + +param set-default FW_R_TC 0.45 +param set-default FW_RR_FF 0.40 +param set-default FW_RR_I 0.132 +param set-default FW_RR_P 0.085 + +param set-default FW_W_EN 1 + +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 20 +param set-default MIS_DIST_1WP 2500 +param set-default MIS_DIST_WPS 10000 + +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +param set-default RWTO_TKOFF 1 + +param set-default RWTO_MAX_PITCH 20 +param set-default RWTO_MAX_ROLL 10 + +param set-default RWTO_PSP 8 +param set-default RWTO_AIRSPD_SCL 1.8 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod index 38183812a7b1..30d7b926b0f9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod @@ -5,43 +5,36 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - param set EKF2_MAG_ACCLIM 0 - param set EKF2_MAG_YAWLIM 0 +param set-default EKF2_MAG_ACCLIM 0 +param set-default EKF2_MAG_YAWLIM 0 - param set FW_LND_AIRSPD_SC 1 - param set FW_LND_ANG 8 - param set FW_THR_LND_MAX 0 +param set-default FW_LND_AIRSPD_SC 1 +param set-default FW_LND_ANG 8 +param set-default FW_THR_LND_MAX 0 - param set FW_L1_PERIOD 15 +param set-default FW_L1_PERIOD 15 - param set FW_P_TC 0.5 - param set FW_PR_FF 0.40 - param set FW_PR_I 0.05 - param set FW_PR_P 0.05 +param set-default FW_P_TC 0.5 +param set-default FW_PR_FF 0.40 +param set-default FW_PR_I 0.05 +param set-default FW_PR_P 0.05 - param set FW_R_TC 0.7 - param set FW_RR_FF 0.20 - param set FW_RR_I 0.02 - param set FW_RR_P 0.22 +param set-default FW_R_TC 0.7 +param set-default FW_RR_FF 0.20 +param set-default FW_RR_I 0.02 +param set-default FW_RR_P 0.22 - param set FW_L1_PERIOD 12 +param set-default FW_L1_PERIOD 12 - param set FW_W_EN 1 +param set-default FW_W_EN 1 - param set MIS_LTRMIN_ALT 30 - param set MIS_TAKEOFF_ALT 30 +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 30 - param set NAV_ACC_RAD 15 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 50 +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 - param set RWTO_TKOFF 1 - -fi +param set-default RWTO_TKOFF 1 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo index 7975bf11c4f2..a6bfc13246a8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo @@ -5,52 +5,41 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - #param set EKF2_MAG_ACCLIM 0 - #param set EKF2_MAG_YAWLIM 0 - - param set FW_LND_AIRSPD_SC 1.1 - param set FW_LND_ANG 5 - param set FW_THR_LND_MAX 0 - param set FW_LND_HHDIST 30 - param set FW_LND_FL_PMIN 9.5 - param set FW_LND_FL_PMAX 20 - param set FW_LND_FLALT 5 - param set FW_LND_TLALT 15 - - param set FW_L1_PERIOD 25 - - param set FW_P_TC 0.4 - param set FW_PR_FF 0.40 - param set FW_PR_I 0.05 - param set FW_PR_P 0.05 - - param set FW_R_TC 0.45 - param set FW_RR_FF 0.40 - param set FW_RR_I 0.132 - param set FW_RR_P 0.085 - - param set FW_W_EN 1 - - param set MIS_LTRMIN_ALT 30 - param set MIS_TAKEOFF_ALT 20 - param set MIS_DIST_1WP 2500 - param set MIS_DIST_WPS 10000 - - param set NAV_ACC_RAD 15 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 50 - - param set RWTO_TKOFF 1 - param set RWTO_MAX_PITCH 20 - param set RWTO_MAX_ROLL 10 - param set RWTO_PSP 8 - param set RWTO_AIRSPD_SCL 1.8 - -fi +param set-default FW_LND_AIRSPD_SC 1.1 +param set-default FW_LND_ANG 5 +param set-default FW_THR_LND_MAX 0 +param set-default FW_LND_HHDIST 30 +param set-default FW_LND_FL_PMIN 9.5 +param set-default FW_LND_FL_PMAX 20 +param set-default FW_LND_FLALT 5 +param set-default FW_LND_TLALT 15 + +param set-default FW_L1_PERIOD 25 + +param set-default FW_PR_FF 0.40 +param set-default FW_PR_I 0.05 +param set-default FW_PR_P 0.05 + +param set-default FW_R_TC 0.45 +param set-default FW_RR_FF 0.40 +param set-default FW_RR_I 0.132 +param set-default FW_RR_P 0.085 + +param set-default FW_W_EN 1 + +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 20 +param set-default MIS_DIST_1WP 2500 +param set-default MIS_DIST_WPS 10000 + +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +param set-default RWTO_TKOFF 1 +param set-default RWTO_MAX_PITCH 20 +param set-default RWTO_MAX_ROLL 10 +param set-default RWTO_PSP 8 +param set-default RWTO_AIRSPD_SCL 1.8 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer new file mode 100644 index 000000000000..1ad89de15746 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer @@ -0,0 +1,52 @@ +#!/bin/sh +# +# @name Plane SITL +# + +. ${R}etc/init.d/rc.fw_defaults + +param set-default EKF2_MAG_ACCLIM 0 +param set-default EKF2_MAG_YAWLIM 0 + +param set-default FW_LND_AIRSPD_SC 1 +param set-default FW_LND_ANG 8 +param set-default FW_THR_LND_MAX 0 + +param set-default FW_L1_PERIOD 12 + +param set-default FW_MAN_P_MAX 30 + +param set-default FW_PR_P 0.9 +param set-default FW_PR_FF 0.2 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MAX 32 +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_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_THR_CRUISE 0.25 + +param set-default FW_T_ALT_TC 2 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 +param set-default FW_T_TAS_TC 2 + +param set-default FW_W_EN 1 + +param set-default MIS_LTRMIN_ALT 30 +param set-default MIS_TAKEOFF_ALT 30 + +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +param set-default FW_USE_NPFG 1 + +param set-default RWTO_TKOFF 1 + +set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix +set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider new file mode 100644 index 000000000000..27f6648911b4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider @@ -0,0 +1,9 @@ +#!/bin/sh +# +# @name Plane SITL with catapult +# + +. ${R}etc/init.d-posix/airframes/1030_plane + +param set-default FW_THR_CRUISE 0.0 +param set-default RWTO_TKOFF 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol index 448d1cc70d40..ff820bd50df2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol @@ -7,52 +7,82 @@ . ${R}etc/init.d/rc.vtol_defaults -if [ $AUTOCNF = yes ] -then - param set FW_L1_PERIOD 12 - param set FW_MAN_P_MAX 30 - param set FW_PR_FF 0.2 - param set FW_PR_I 0.4 - param set FW_PR_P 0.9 - param set FW_PSP_OFF 2 - param set FW_P_LIM_MAX 32 - param set FW_P_LIM_MIN -15 - param set FW_RR_FF 0.1 - param set FW_RR_P 0.3 - param set FW_THR_CRUISE 0.25 - param set FW_THR_MAX 0.6 - param set FW_THR_MIN 0.05 - param set FW_T_ALT_TC 2 - param set FW_T_CLMB_MAX 8 - param set FW_T_HRATE_FF 0.5 - param set FW_T_SINK_MAX 2.7 - param set FW_T_SINK_MIN 2.2 - param set FW_T_TAS_TC 2 - - param set MC_ROLLRATE_P 0.3 - param set MC_YAW_P 1.6 - - param set MIS_TAKEOFF_ALT 10 - - param set MPC_ACC_HOR_MAX 2 - param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P_ACC 3 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_D_ACC 0.1 - - param set NAV_ACC_RAD 5 - param set NAV_LOITER_RAD 80 - - param set VT_FWD_THRUST_EN 4 - param set VT_F_TRANS_THR 0.75 - param set VT_MOT_ID 1234 - param set VT_FW_MOT_OFFID 1234 - param set VT_B_TRANS_DUR 8 - param set VT_TYPE 2 - -fi - -set MAV_TYPE 22 +# TODO: Enable motor failure detection when the +# VTOL no longer reports 0A for all ESCs in SITL +param set-default FD_ACT_EN 0 +param set-default FD_ACT_MOT_TOUT 500 + +# param set-default SYS_CTRL_ALLOC 1 +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.0 +param set-default CA_ROTOR4_AZ 0.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.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 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_FUNC8 203 + +param set-default FW_L1_PERIOD 12 +param set-default FW_MAN_P_MAX 30 +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_MAX 32 +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_CRUISE 0.25 +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_ROLLRATE_P 0.3 +param set-default MC_YAW_P 1.6 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default MPC_ACC_HOR_MAX 2 +param set-default MPC_XY_P 0.8 +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 NAV_ACC_RAD 5 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_F_TRANS_THR 0.75 +param set-default VT_MOT_ID 1234 +param set-default VT_FW_MOT_OFFID 1234 +param set-default VT_B_TRANS_DUR 8 +param set-default VT_TYPE 2 set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter index 8adffd9ab29d..e5f809af7460 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter @@ -7,46 +7,77 @@ . ${R}etc/init.d/rc.vtol_defaults -if [ $AUTOCNF = yes ] -then - param set FW_L1_PERIOD 12 - param set FW_MAN_P_MAX 30 - param set FW_PR_I 0.2 - param set FW_PR_P 0.3 - param set FW_PSP_OFF 2 - param set FW_P_LIM_MAX 32 - param set FW_P_LIM_MIN -15 - param set FW_RR_P 0.3 - param set FW_THR_CRUISE 0.33 - param set FW_THR_MAX 0.6 - param set FW_THR_MIN 0.05 - param set FW_T_ALT_TC 2 - param set FW_T_CLMB_MAX 8 - param set FW_T_HRATE_FF 0.5 - param set FW_T_SINK_MAX 2.7 - param set FW_T_SINK_MIN 2.2 - param set FW_T_TAS_TC 2 - - param set MC_ROLLRATE_P 0.3 - - param set MPC_ACC_HOR_MAX 2 - param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P_ACC 3 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_D_ACC 0.1 - - param set NAV_ACC_RAD 5 - param set NAV_LOITER_RAD 80 - - param set VT_F_TRANS_DUR 1.5 - param set VT_F_TRANS_THR 0.7 - param set VT_TYPE 0 - - param set WV_EN 0 - -fi - -set MAV_TYPE 20 +param set-default MAV_TYPE 20 + +# param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 1 +param set-default CA_ROTOR0_PY 2 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -1 +param set-default CA_ROTOR1_PY -1 +param set-default CA_ROTOR1_KM 0.05 +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 2 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_Y -0.5 +param set-default CA_SV_CS1_TYPE 6 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_Y 0.5 + +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 0 +param set-default PWM_MAIN_FUNC6 201 +param set-default PWM_MAIN_FUNC7 202 +param set-default PWM_MAIN_REV 96 # invert both elevons + +param set-default FW_L1_PERIOD 12 +param set-default FW_MAN_P_MAX 30 +param set-default FW_PR_I 0.2 +param set-default FW_PR_P 0.2 +param set-default FW_PSP_OFF 2 +param set-default FW_P_LIM_MAX 32 +param set-default FW_P_LIM_MIN -15 +param set-default FW_RR_P 0.2 +param set-default FW_THR_CRUISE 0.33 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_ALT_TC 2 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 +param set-default FW_T_TAS_TC 2 + +param set-default MC_ROLLRATE_P 0.3 + +param set-default MPC_ACC_HOR_MAX 2 +param set-default MPC_XY_P 0.8 +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 NAV_ACC_RAD 5 + +param set-default VT_FW_DIFTHR_EN 1 +param set-default VT_FW_DIFTHR_SC 0.5 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_F_TRANS_THR 0.7 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 set MIXER_FILE etc/mixers-sitl/quad_x_vtol.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor index 0b3076ef60d5..81af841a6da8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor @@ -7,50 +7,85 @@ . ${R}etc/init.d/rc.vtol_defaults -if [ $AUTOCNF = yes ] -then - param set FW_L1_PERIOD 12 - param set FW_MAN_P_MAX 30 - param set FW_PR_FF 0.2 - param set FW_PR_I 0.4 - param set FW_PR_P 0.9 - param set FW_PSP_OFF 2 - param set FW_P_LIM_MAX 32 - param set FW_P_LIM_MIN -15 - param set FW_RR_FF 0.1 - param set FW_RR_P 0.3 - param set FW_THR_CRUISE 0.38 - param set FW_THR_MAX 0.6 - param set FW_THR_MIN 0.05 - param set FW_T_ALT_TC 2 - param set FW_T_CLMB_MAX 8 - param set FW_T_HRATE_FF 0.5 - param set FW_T_SINK_MAX 2.7 - param set FW_T_SINK_MIN 2.2 - param set FW_T_TAS_TC 2 - - param set MC_YAW_P 1.6 - - param set MIS_TAKEOFF_ALT 10 - - param set MPC_ACC_HOR_MAX 2 - param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P_ACC 3 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_D_ACC 0.1 - - param set NAV_ACC_RAD 5 - param set NAV_LOITER_RAD 80 - - param set VT_B_TRANS_DUR 8 - param set VT_FWD_THRUST_EN 4 - param set VT_MOT_ID 1234 - param set VT_TILT_TRANS 0.6 - param set VT_TYPE 1 - -fi - -set MAV_TYPE 21 +param set-default MAV_TYPE 21 + +param set-default SYS_CTRL_ALLOC 1 +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 3 +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.0 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_TL0_CT 0 +param set-default CA_SV_TL1_CT 0 +param set-default CA_SV_TL2_CT 0 +param set-default CA_SV_TL3_CT 0 +param set-default CA_SV_TL_COUNT 4 + +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 204 +param set-default PWM_MAIN_FUNC6 205 +param set-default PWM_MAIN_FUNC7 206 +param set-default PWM_MAIN_FUNC8 207 +param set-default PWM_MAIN_FUNC9 201 +param set-default PWM_MAIN_FUNC10 202 +param set-default PWM_MAIN_FUNC11 203 + +param set-default FW_L1_PERIOD 12 +param set-default FW_MAN_P_MAX 30 +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_MAX 32 +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_CRUISE 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_YAW_P 1.6 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default MPC_ACC_HOR_MAX 2 +param set-default MPC_XY_P 0.8 +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 NAV_ACC_RAD 5 + +param set-default VT_B_TRANS_DUR 8 +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_MOT_ID 1234 +param set-default VT_TILT_TRANS 0.6 +param set-default VT_TYPE 1 set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop index 8dcb6fd26294..d4a274d2ba35 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop @@ -7,55 +7,48 @@ . ${R}etc/init.d/rc.vtol_defaults -if [ $AUTOCNF = yes ] -then - param set FW_L1_PERIOD 12 - param set FW_MAN_P_MAX 30 - param set FW_PR_FF 0.2 - param set FW_PR_I 0.4 - param set FW_PR_P 0.9 - param set FW_PSP_OFF 2 - param set FW_P_LIM_MAX 32 - param set FW_P_LIM_MIN -15 - param set FW_RR_FF 0.1 - param set FW_RR_P 0.3 - param set FW_THR_CRUISE 0.25 - param set FW_THR_MAX 0.6 - param set FW_THR_MIN 0.05 - param set FW_T_ALT_TC 2 - param set FW_T_CLMB_MAX 8 - param set FW_T_HRATE_FF 0.5 - param set FW_T_SINK_MAX 2.7 - param set FW_T_SINK_MIN 2.2 - param set FW_T_TAS_TC 2 - - param set MC_ROLLRATE_P 0.3 - param set MC_YAW_P 1.6 - - param set MIS_TAKEOFF_ALT 10 - - param set MPC_ACC_HOR_MAX 2 - param set MPC_XY_P 0.8 - param set MPC_XY_VEL_P_ACC 3 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_D_ACC 0.1 - - param set NAV_ACC_RAD 5 - param set NAV_LOITER_RAD 80 - - param set VT_FWD_THRUST_EN 4 - param set VT_F_TRANS_THR 0.75 - param set VT_MOT_ID 1234 - param set VT_FW_MOT_OFFID 1234 - param set VT_B_TRANS_DUR 8 - param set VT_TYPE 2 - - param set RC_MAP_AUX1 8 - param set RC_MAP_AUX2 9 - param set RC_MAP_AUX3 10 -fi - -set MAV_TYPE 22 +param set-default FW_L1_PERIOD 12 +param set-default FW_MAN_P_MAX 30 +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_MAX 32 +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_CRUISE 0.25 +param set-default FW_THR_MAX 0.6 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_ALT_TC 2 +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 +param set-default FW_T_TAS_TC 2 + +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_YAW_P 1.6 + +param set-default MIS_TAKEOFF_ALT 10 + +param set-default MPC_ACC_HOR_MAX 2 +param set-default MPC_XY_P 0.8 +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 NAV_ACC_RAD 5 + +param set-default VT_FWD_THRUST_EN 4 +param set-default VT_F_TRANS_THR 0.75 +param set-default VT_MOT_ID 1234 +param set-default VT_FW_MOT_OFFID 1234 +param set-default VT_B_TRANS_DUR 8 +param set-default VT_TYPE 2 + +param set-default RC_MAP_AUX1 8 +param set-default RC_MAP_AUX2 9 +param set-default RC_MAP_AUX3 10 set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index ccf86b9f8adb..ad1727bb33e1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover @@ -5,33 +5,36 @@ . ${R}etc/init.d/rc.rover_defaults -if [ $AUTOCNF = yes ] -then - param set GND_L1_DIST 5 - param set GND_L1_PERIOD 10 - param set GND_SP_CTRL_MODE 1 - param set GND_SPEED_D 0.001 - param set GND_SPEED_I 3 - param set GND_SPEED_IMAX 0.125 - param set GND_SPEED_P 0.25 - param set GND_SPEED_THR_SC 1 - param set GND_SPEED_TRIM 4 - param set GND_THR_CRUISE 0.3 - param set GND_THR_MAX 0.5 - param set GND_THR_MIN 0 - - param set MIS_LTRMIN_ALT 0.01 - param set MIS_TAKEOFF_ALT 0.01 - param set NAV_ACC_RAD 0.5 - param set NAV_LOITER_RAD 2 - - param set CBRK_AIRSPD_CHK 162128 - - param set GND_MAX_ANG 0.6 - param set GND_WHEEL_BASE 2.0 - -fi - -set MAV_TYPE 10 - -set MIXER_FILE etc/mixers-sitl/rover_ackermann_sitl.main.mix +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 10 +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_I 3 +param set-default GND_SPEED_IMAX 0.125 +param set-default GND_SPEED_P 0.25 +param set-default GND_SPEED_THR_SC 1 +param set-default GND_SPEED_TRIM 4 +param set-default GND_THR_CRUISE 0.3 +param set-default GND_THR_MAX 0.5 +param set-default GND_THR_MIN 0 + +param set-default MIS_LTRMIN_ALT 0.01 +param set-default MIS_TAKEOFF_ALT 0.01 +param set-default NAV_ACC_RAD 0.5 +param set-default NAV_LOITER_RAD 2 + +param set-default CBRK_AIRSPD_CHK 162128 + +param set-default GND_MAX_ANG 0.6 +param set-default GND_WHEEL_BASE 2.0 + +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 5 + +param set-default CA_R_REV 1 +param set-default PWM_MAIN_FUNC1 201 +param set-default PWM_MAIN_FUNC2 201 +param set-default PWM_MAIN_FUNC6 101 +param set-default PWM_MAIN_FUNC7 101 + +set MIXER_FILE skip diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover index ddd04393a104..2be17c126214 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover @@ -6,32 +6,35 @@ . ${R}etc/init.d/rc.rover_defaults -if [ $AUTOCNF = yes ] -then - param set GND_L1_DIST 5 - param set GND_SP_CTRL_MODE 1 - param set GND_SPEED_D 3 - param set GND_SPEED_I 0.001 - param set GND_SPEED_IMAX 0.125 - param set GND_SPEED_P 0.25 - param set GND_SPEED_THR_SC 1 - param set GND_SPEED_TRIM 4 - param set GND_THR_CRUISE 0.3 - param set GND_THR_MAX 0.5 - param set GND_THR_MIN 0 - - param set MIS_LTRMIN_ALT 0.01 - param set MIS_TAKEOFF_ALT 0.01 - param set NAV_ACC_RAD 0.5 - param set NAV_LOITER_RAD 2 - - param set CBRK_AIRSPD_CHK 162128 - - param set GND_MAX_ANG 0.6 - param set GND_WHEEL_BASE 2.0 - -fi - -set MAV_TYPE 10 - -set MIXER_FILE etc/mixers-sitl/rover_diff_sitl.main.mix +param set-default GND_L1_DIST 5 +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_SPEED_D 3 +param set-default GND_SPEED_I 0.001 +param set-default GND_SPEED_IMAX 0.125 +param set-default GND_SPEED_P 0.25 +param set-default GND_SPEED_THR_SC 1 +param set-default GND_SPEED_TRIM 4 +param set-default GND_THR_CRUISE 0.3 +param set-default GND_THR_MAX 0.5 +param set-default GND_THR_MIN 0 + +param set-default MIS_LTRMIN_ALT 0.01 +param set-default MIS_TAKEOFF_ALT 0.01 +param set-default NAV_ACC_RAD 0.5 +param set-default NAV_LOITER_RAD 2 + +param set-default CBRK_AIRSPD_CHK 162128 + +param set-default GND_MAX_ANG 0.6 +param set-default GND_WHEEL_BASE 2.0 + +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 6 + +param set-default CA_R_REV 3 +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 101 +param set-default PWM_MAIN_FUNC6 102 +param set-default PWM_MAIN_FUNC7 102 + +set MIXER_FILE skip diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 index 43743a2be9a0..4e271c77b95c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 @@ -10,33 +10,29 @@ . ${R}etc/init.d/rc.rover_defaults -if [ $AUTOCNF = yes ] -then - param set GND_L1_DIST 5 - param set GND_SP_CTRL_MODE 1 - param set GND_SPEED_D 3 - param set GND_SPEED_I 0.001 - param set GND_SPEED_IMAX 0.125 - param set GND_SPEED_P 0.25 - param set GND_SPEED_THR_SC 1 - param set GND_SPEED_TRIM 15 - param set GND_SPEED_MAX 25 - param set GND_THR_CRUISE 0.3 - param set GND_THR_MAX 0.5 - param set GND_THR_MIN 0 - - param set MIS_LTRMIN_ALT 0.01 - param set MIS_TAKEOFF_ALT 0.01 - param set NAV_ACC_RAD 0.5 - param set NAV_LOITER_RAD 2 - - param set CBRK_AIRSPD_CHK 162128 - - param set GND_MAX_ANG 0.6 - param set GND_WHEEL_BASE 3.0 - -fi - -set MAV_TYPE 10 +param set-default MAV_TYPE 10 + +param set-default GND_L1_DIST 5 +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_SPEED_D 3 +param set-default GND_SPEED_I 0.001 +param set-default GND_SPEED_IMAX 0.125 +param set-default GND_SPEED_P 0.25 +param set-default GND_SPEED_THR_SC 1 +param set-default GND_SPEED_TRIM 15 +param set-default GND_SPEED_MAX 25 +param set-default GND_THR_CRUISE 0.3 +param set-default GND_THR_MAX 0.5 +param set-default GND_THR_MIN 0 + +param set-default MIS_LTRMIN_ALT 0.01 +param set-default MIS_TAKEOFF_ALT 0.01 +param set-default NAV_ACC_RAD 0.5 +param set-default NAV_LOITER_RAD 2 + +param set-default CBRK_AIRSPD_CHK 162128 + +param set-default GND_MAX_ANG 0.6 +param set-default GND_WHEEL_BASE 3.0 set MIXER_FILE etc/mixers-sitl/rover_ackermann_sitl.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat index a8c1b52f74ab..c45d9d78b9c7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat @@ -5,33 +5,46 @@ . ${R}etc/init.d/rc.boat_defaults -if [ $AUTOCNF = yes ] -then - param set GND_L1_DIST 5 - param set GND_L1_PERIOD 100 - param set GND_SP_CTRL_MODE 1 - param set GND_SPEED_D 0.001 - param set GND_SPEED_I 8 - param set GND_SPEED_IMAX 0.125 - param set GND_SPEED_P 2 - param set GND_SPEED_THR_SC 1 - param set GND_SPEED_TRIM 1 - param set GND_THR_CRUISE 0.85 - param set GND_THR_MAX 1 - param set GND_THR_MIN 0 - - param set MIS_LTRMIN_ALT 0.01 - param set MIS_TAKEOFF_ALT 0.01 - param set NAV_ACC_RAD 0.5 - param set NAV_LOITER_RAD 2 - - param set CBRK_AIRSPD_CHK 162128 - - param set GND_MAX_ANG 0.6 - param set GND_WHEEL_BASE 2.0 - -fi - -set MAV_TYPE 11 - -set MIXER_FILE etc/mixers-sitl/boat_sitl.main.mix +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 100 +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_I 8 +param set-default GND_SPEED_IMAX 0.125 +param set-default GND_SPEED_P 2 +param set-default GND_SPEED_THR_SC 1 +param set-default GND_SPEED_TRIM 1 +param set-default GND_THR_CRUISE 0.85 +param set-default GND_THR_MAX 1 +param set-default GND_THR_MIN 0 + +param set-default MIS_LTRMIN_ALT 0.01 +param set-default MIS_TAKEOFF_ALT 0.01 +param set-default NAV_ACC_RAD 0.5 +param set-default NAV_LOITER_RAD 2 + +param set-default CBRK_AIRSPD_CHK 162128 + +param set-default GND_MAX_ANG 0.6 +param set-default GND_WHEEL_BASE 2.0 + +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 9 + +param set-default CA_ROTOR_COUNT 2 +param set-default CA_ROTOR0_AX 1 +param set-default CA_ROTOR0_AZ 0 +param set-default CA_ROTOR0_KM 0 +param set-default CA_ROTOR0_PX -2 +param set-default CA_ROTOR0_PY -1 +param set-default CA_ROTOR1_AX 1 +param set-default CA_ROTOR1_AZ 0 +param set-default CA_ROTOR1_KM 0 +param set-default CA_ROTOR1_PX -2 +param set-default CA_ROTOR1_PY 1 +param set-default CA_R_REV 3 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 + +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 index fa7d78da3109..117041cae4b4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 @@ -12,52 +12,38 @@ . ${R}etc/init.d/rc.fw_defaults -if [ $AUTOCNF = yes ] -then - param set EKF2_ARSP_THR 8 - param set EKF2_FUSE_BETA 1 - param set ASPD_STALL 10.0 +param set-default FW_AIRSPD_STALL 8 - param set FW_P_RMAX_NEG 20.0 - param set FW_P_RMAX_POS 60.0 - param set FW_W_RMAX 10 - param set FW_W_EN 1 +param set-default FW_P_RMAX_NEG 20.0 +param set-default FW_W_RMAX 10 +param set-default FW_W_EN 1 - param set FW_PR_IMAX 0.4 - param set FW_R_TC 0.4 - param set FW_RR_FF 0.5 - param set FW_RR_I 0.1 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.08 +param set-default FW_RR_P 0.08 - param set MIS_LTRMIN_ALT 50 - param set MIS_TAKEOFF_ALT 3 +param set-default MIS_LTRMIN_ALT 50 +param set-default MIS_TAKEOFF_ALT 3 - param set NAV_ACC_RAD 20 - param set NAV_DLL_ACT 2 - param set NAV_LOITER_RAD 100 +param set-default NAV_ACC_RAD 20 +param set-default NAV_DLL_ACT 2 +param set-default NAV_LOITER_RAD 100 - param set RWTO_TKOFF 1 +param set-default RWTO_TKOFF 1 - param set FW_ARSP_SCALE_EN 0 +param set-default FW_ARSP_SCALE_EN 0 - param set FW_AIRSPD_MAX 35 - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 15 +param set-default FW_AIRSPD_MAX 35 +param set-default FW_AIRSPD_MIN 7 - param set FW_P_LIM_MAX 25 - param set FW_P_LIM_MIN -5 - param set FW_R_LIM 30 +param set-default FW_P_LIM_MAX 25 +param set-default FW_P_LIM_MIN -5 +param set-default FW_R_LIM 30 - param set FW_MAN_P_MAX 30.0 - param set FW_MAN_R_MAX 30.0 - - param set FW_THR_CRUISE 0.8 - param set FW_THR_IDLE 0 - param set COM_DISARM_PRFLT 0 - -fi +param set-default FW_MAN_P_MAX 30.0 +param set-default FW_MAN_R_MAX 30.0 +param set-default FW_THR_CRUISE 0.8 +param set-default FW_THR_IDLE 0 +param set-default COM_DISARM_PRFLT 0 set MIXER_FILE etc/mixers-sitl/autogyro_sitl.main.mix set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_cloudship index 09bfb9b5285c..33be5fa87096 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_cloudship @@ -11,5 +11,31 @@ . ${R}etc/init.d/rc.airship_defaults -set MIXER cloudship -set PWM_OUT 1234 +param set-default SYS_CTRL_ALLOC 1 +param set-default CA_AIRFRAME 9 + +param set-default CA_ROTOR_COUNT 3 +param set-default CA_ROTOR0_AX 1.0000 +param set-default CA_ROTOR0_AZ 0.0000 +param set-default CA_ROTOR0_KM 0.0000 +param set-default CA_ROTOR0_PY 2.0000 +param set-default CA_ROTOR1_AX 1.0000 +param set-default CA_ROTOR1_AZ 0.0000 +param set-default CA_ROTOR1_KM 0.0000 +param set-default CA_ROTOR1_PY -2.0000 +param set-default CA_ROTOR2_AY -1.0000 +param set-default CA_ROTOR2_AZ 0.0000 +param set-default CA_ROTOR2_KM 0.0000 +param set-default CA_ROTOR2_PX -10.0000 + +param set-default CA_SV_CS_COUNT 1 +param set-default CA_SV_CS0_TRQ_P 1.0000 + +param set-default CA_R_REV 7 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 201 +param set-default PWM_MAIN_FUNC4 103 + +set MIXER skip diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x index 36086a09f211..85f14ef4d6b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x @@ -9,25 +9,21 @@ . ${R}etc/init.d/rc.mc_defaults -if [ $AUTOCNF = yes ] -then - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.05 - param set MC_PITCH_P 6.0 - param set MC_ROLLRATE_P 0.15 - param set MC_ROLLRATE_I 0.1 - param set MC_ROLL_P 6.0 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_P_ACC 3 +param set-default MAV_TYPE 13 - param set RTL_DESCEND_ALT 10 +param set-default MC_PITCHRATE_P 0.1 +param set-default MC_PITCHRATE_I 0.05 +param set-default MC_PITCH_P 6.0 +param set-default MC_ROLLRATE_I 0.1 +param set-default MC_ROLL_P 6.0 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_P_ACC 3 - param set TRIG_INTERFACE 3 - param set TRIG_MODE 4 - param set MNT_MODE_IN 4 - param set MNT_DO_STAB 2 -fi +param set-default RTL_DESCEND_ALT 10 -set MAV_TYPE 13 +param set-default TRIG_INTERFACE 3 +param set-default TRIG_MODE 4 +param set-default MNT_MODE_IN 4 +param set-default MNT_DO_STAB 2 set MIXER hexa_x diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 index 790df529354b..2cdd4a53142e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 @@ -7,28 +7,25 @@ . ${R}etc/init.d/rc.mc_defaults -if [ $AUTOCNF = yes ] -then - param set MC_PITCHRATE_P 0.0800 - param set MC_PITCHRATE_I 0.0400 - param set MC_PITCHRATE_D 0.0010 - param set MC_PITCH_P 9.0 - param set MC_ROLLRATE_P 0.0800 - param set MC_ROLLRATE_I 0.0400 - param set MC_ROLLRATE_D 0.0010 - param set MC_ROLL_P 9.0 - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_P_ACC 3 +param set-default MAV_TYPE 13 - param set RTL_DESCEND_ALT 10 +param set-default MC_PITCHRATE_P 0.0800 +param set-default MC_PITCHRATE_I 0.0400 +param set-default MC_PITCHRATE_D 0.0010 +param set-default MC_PITCH_P 9.0 +param set-default MC_ROLLRATE_P 0.0800 +param set-default MC_ROLLRATE_I 0.0400 +param set-default MC_ROLLRATE_D 0.0010 +param set-default MC_ROLL_P 9.0 +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_P_ACC 3 - param set TRIG_INTERFACE 3 - param set TRIG_MODE 4 - param set MNT_MODE_IN 4 - param set MNT_MODE_OUT 2 - param set MAV_PROTO_VER 2 -fi +param set-default RTL_DESCEND_ALT 10 -set MAV_TYPE 13 +param set-default TRIG_INTERFACE 3 +param set-default TRIG_MODE 4 +param set-default MNT_MODE_IN 4 +param set-default MNT_MODE_OUT 2 +param set-default MAV_PROTO_VER 2 set MIXER hexa_x diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc deleted file mode 100644 index 843a8f01db59..000000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc +++ /dev/null @@ -1,75 +0,0 @@ -#!/bin/sh -# -# @name Typhoon H480 SITL -# -# @type Hexarotor x -# - -. ${R}etc/init.d/rc.mc_defaults -. ${R}etc/init.d/rc.ctrlalloc - -if [ $AUTOCNF = yes ] -then - param set MPC_XY_VEL_I_ACC 4 - param set MPC_XY_VEL_P_ACC 3 - - param set RTL_DESCEND_ALT 10 - param set RTL_LAND_DELAY 0 - - param set TRIG_INTERFACE 3 - param set TRIG_MODE 4 - param set MNT_MODE_IN 0 - param set MAV_PROTO_VER 2 - - param set MPC_USE_HTE 0 - - param set VM_MASS 2.66 - param set VM_INERTIA_XX 0.06 - param set VM_INERTIA_YY 0.06 - param set VM_INERTIA_ZZ 0.10 - - param set CA_AIRFRAME 0 - param set CA_METHOD 1 - param set CA_ACT0_MIN 0.0 - param set CA_ACT1_MIN 0.0 - param set CA_ACT2_MIN 0.0 - param set CA_ACT3_MIN 0.0 - param set CA_ACT4_MIN 0.0 - param set CA_ACT5_MIN 0.0 - param set CA_ACT0_MAX 1.0 - param set CA_ACT1_MAX 1.0 - param set CA_ACT2_MAX 1.0 - param set CA_ACT3_MAX 1.0 - param set CA_ACT4_MAX 1.0 - param set CA_ACT5_MAX 1.0 - - param set CA_MC_R0_PX 0.0 - param set CA_MC_R0_PY 1.0 - param set CA_MC_R0_CT 9.5 - param set CA_MC_R0_KM -0.05 - param set CA_MC_R1_PX 0.0 - param set CA_MC_R1_PY -1.0 - param set CA_MC_R1_CT 9.5 - param set CA_MC_R1_KM 0.05 - param set CA_MC_R2_PX 0.866025 - param set CA_MC_R2_PY -0.5 - param set CA_MC_R2_CT 9.5 - param set CA_MC_R2_KM -0.05 - param set CA_MC_R3_PX -0.866025 - param set CA_MC_R3_PY 0.5 - param set CA_MC_R3_CT 9.5 - param set CA_MC_R3_KM 0.05 - param set CA_MC_R4_PX 0.866025 - param set CA_MC_R4_PY 0.5 - param set CA_MC_R4_CT 9.5 - param set CA_MC_R4_KM 0.05 - param set CA_MC_R5_PX -0.866025 - param set CA_MC_R5_PY -0.5 - param set CA_MC_R5_CT 9.5 - param set CA_MC_R5_KM -0.05 -fi - -set MAV_TYPE 13 - -# set MIXER hexa_x -set MIXER direct diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post deleted file mode 100644 index 0af01438b74d..000000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post +++ /dev/null @@ -1,10 +0,0 @@ - -mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix - -mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p - -# shellcheck disable=SC2154 -mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local -# shellcheck disable=SC2154 -mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local -mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc new file mode 100644 index 000000000000..39e360f8be82 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc @@ -0,0 +1,64 @@ +#!/bin/sh +# +# @name Typhoon H480 SITL +# +# @type Hexarotor x +# + +. ${R}etc/init.d/rc.mc_defaults + +param set-default MAV_TYPE 13 + +param set-default SYS_CTRL_ALLOC 1 + +param set-default MC_PITCHRATE_P 0.0800 +param set-default MC_PITCHRATE_I 0.0400 +param set-default MC_PITCHRATE_D 0.0010 +param set-default MC_PITCH_P 9.0 +param set-default MC_ROLLRATE_P 0.0800 +param set-default MC_ROLLRATE_I 0.0400 +param set-default MC_ROLLRATE_D 0.0010 +param set-default MC_ROLL_P 9.0 + +param set-default MPC_XY_VEL_I_ACC 4 +param set-default MPC_XY_VEL_P_ACC 3 + +param set-default RTL_DESCEND_ALT 10 +param set-default RTL_LAND_DELAY 0 + +param set-default TRIG_INTERFACE 3 +param set-default TRIG_MODE 4 +param set-default MNT_MODE_IN 0 +param set-default MAV_PROTO_VER 2 + +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 6 + +param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PY 1.0 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PY -1.0 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.866025 +param set-default CA_ROTOR2_PY -0.5 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.866025 +param set-default CA_ROTOR3_PY 0.5 +param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR4_PX 0.866025 +param set-default CA_ROTOR4_PY 0.5 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR5_PX -0.866025 +param set-default CA_ROTOR5_PY -0.5 +param set-default CA_ROTOR5_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 PWM_MAIN_FUNC5 105 +param set-default PWM_MAIN_FUNC6 106 + +set MIXER skip +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post new file mode 100644 index 000000000000..13788654ebe7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc.post @@ -0,0 +1,8 @@ + +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p + +# shellcheck disable=SC2154 +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local +# shellcheck disable=SC2154 +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local +mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 6cef7670ce50..fba035b61e82 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -35,8 +35,12 @@ px4_add_romfs_files( 10016_iris 10017_iris_ctrlalloc 10018_iris_foggy_lidar + 10019_omnicopter 10020_if750a 10030_px4vision + 10040_quadx + 10041_airplane + 10042_xvert 1010_iris_opt_flow 1010_iris_opt_flow.post 1011_iris_irlock @@ -46,8 +50,6 @@ px4_add_romfs_files( 1014_solo 1015_iris_obs_avoid 1015_iris_obs_avoid.post - 1016_iris_rtps - 1016_iris_rtps.post 1017_iris_opt_flow_mockup 1018_iris_vision_velocity 1019_iris_dual_gps @@ -62,6 +64,8 @@ px4_add_romfs_files( 1034_rascal-electric 1035_techpod 1036_malolo + 1037_believer + 1038_glider 1040_standard_vtol 1041_tailsitter 1042_tiltrotor @@ -77,6 +81,6 @@ px4_add_romfs_files( 2507_cloudship 6011_typhoon_h480 6011_typhoon_h480.post - 6012_typhoon_ctrlalloc - 6012_typhoon_ctrlalloc.post + 6012_typhoon_h480_ctrlalloc + 6012_typhoon_h480_ctrlalloc.post ) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink new file mode 100644 index 000000000000..0867fa5c64ea --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink @@ -0,0 +1,41 @@ +#!/bin/sh +# shellcheck disable=SC2154 + +udp_offboard_port_local=$((14580+px4_instance)) +udp_offboard_port_remote=$((14540+px4_instance)) +[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps +udp_onboard_payload_port_local=$((14280+px4_instance)) +udp_onboard_payload_port_remote=$((14030+px4_instance)) +udp_onboard_gimbal_port_local=$((13030+px4_instance)) +udp_onboard_gimbal_port_remote=$((13280+px4_instance)) +udp_gcs_port_local=$((18570+px4_instance)) + +# GCS link +mavlink start -x -u $udp_gcs_port_local -r 4000000 -f +mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local +mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local +mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local +mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local +mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local +mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local + +# API/Offboard link +mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote + +# Onboard link to camera +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 + +# To display for SIH sitl +if [ "$SIM_MODE" = "sihsim" ]; then + udp_sihsim_port_local=$((19450+px4_instance)) + udp_sihsim_port_remote=$((19410+px4_instance)) + mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote + mavlink stream -r 200 -s HIL_ACTUATOR_CONTROLS -u $udp_sihsim_port_local + mavlink stream -r 25 -s HIL_STATE_QUATERNION -u $udp_sihsim_port_local +fi diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params new file mode 100644 index 000000000000..d05f50146a42 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params @@ -0,0 +1,5 @@ +#!/bin/sh +# shellcheck disable=SC2154 + +#param set-default MAV_SYS_ID $((px4_instance+1)) +#param set-default IMU_INTEG_RATE 250 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_iris_rtps.post b/ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps similarity index 100% rename from ROMFS/px4fmu_common/init.d-posix/airframes/1016_iris_rtps.post rename to ROMFS/px4fmu_common/init.d-posix/px4-rc.rtps diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator new file mode 100644 index 000000000000..cb00e937fc90 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -0,0 +1,20 @@ +#!/bin/sh +# shellcheck disable=SC2154 + +simulator_tcp_port=$((4560+px4_instance)) + +# Check if PX4_SIM_HOSTNAME environment variable is empty +# If empty check if PX4_SIM_HOST_ADDR environment variable is empty +# If both are empty use localhost for simulator +if [ -z "${PX4_SIM_HOSTNAME}" ]; then + if [ -z "${PX4_SIM_HOST_ADDR}" ]; then + echo "PX4 SIM HOST: localhost" + simulator start -c $simulator_tcp_port + else + echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" + simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port + fi +else + echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" + simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port +fi diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay b/ROMFS/px4fmu_common/init.d-posix/rc.replay index 89c999a38c7f..ea0510280f57 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rc.replay +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay @@ -10,7 +10,7 @@ fi if [ ! -f replay_params.txt ]; then echo "Creating $(pwd)/replay_params.txt" - ulog_params -i "${replay}" -d ' ' | grep -e '^EKF2' > replay_params.txt + ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt fi publisher_rules_file="orb_publisher.rules" @@ -21,6 +21,7 @@ ignore_others: false EOF param set SDLOG_DIRS_MAX 7 +param set SDLOG_PROFILE 3 # apply all params before ekf starts, as some params cannot be changed after startup replay tryapplyparams diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 19fea1a602d5..25dc9c2fcf55 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -1,11 +1,14 @@ #!/bin/sh +set -e + # PX4 commands need the 'px4-' prefix in bash. # (px4-alias.sh is expected to be in the PATH) # shellcheck disable=SC1091 . px4-alias.sh -SCRIPT_DIR="$(CDPATH='' cd -- "$(dirname -- "$0")" && pwd)" +#search path for sourcing px4-rc.* +PATH="$PATH:${R}etc/init.d-posix" # # Main SITL startup script @@ -20,10 +23,7 @@ then fi # initialize script variables -set AUX_MODE none -set AUX_BANK2 none set IO_PRESENT no -set MAV_TYPE none set MIXER none set MIXER_AUX none set MIXER_FILE none @@ -45,12 +45,12 @@ else # Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL}) # TODO: unify with rc.autostart generation # shellcheck disable=SC2012 - REQUESTED_AUTOSTART=$(ls "$SCRIPT_DIR/airframes" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p') + REQUESTED_AUTOSTART=$(ls "${R}etc/init.d-posix/airframes" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p') if [ -z "$REQUESTED_AUTOSTART" ]; then - echo "Error: Unknown model $PX4_SIM_MODEL (not found by name on $SCRIPT_DIR/airframes)" + echo "ERROR [init] Unknown model $PX4_SIM_MODEL (not found by name on ${R}etc/init.d-posix/airframes)" exit 1 else - echo "Info: found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART" + echo "INFO [init] found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART" fi fi @@ -95,24 +95,14 @@ fi if param compare SYS_AUTOCONFIG 1 then + # Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID. + param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT* set AUTOCNF yes - - # Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID - param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT* fi # multi-instance setup # shellcheck disable=SC2154 param set MAV_SYS_ID $((px4_instance+1)) -simulator_tcp_port=$((4560+px4_instance)) -udp_offboard_port_local=$((14580+px4_instance)) -udp_offboard_port_remote=$((14540+px4_instance)) -[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps -udp_onboard_payload_port_local=$((14280+px4_instance)) -udp_onboard_payload_port_remote=$((14030+px4_instance)) -udp_onboard_gimbal_port_local=$((13030+px4_instance)) -udp_onboard_gimbal_port_remote=$((13280+px4_instance)) -udp_gcs_port_local=$((18570+px4_instance)) if [ $AUTOCNF = yes ] then @@ -130,11 +120,9 @@ then param set SENS_BOARD_X_OFF 0.000001 param set SENS_DPRES_OFF 0.001 - - param set SYS_RESTART_TYPE 2 fi -param set-default BAT_N_CELLS 4 +param set-default BAT1_N_CELLS 4 param set-default CBRK_AIRSPD_CHK 0 param set-default CBRK_SUPPLY_CHK 894281 @@ -156,6 +144,8 @@ param set-default SENS_MAG_MODE 0 param set-default IMU_GYRO_FFT_EN 1 +param set-default -s MC_AT_EN 1 + # By default log from boot until first disarm. param set-default SDLOG_MODE 1 # enable default, estimator replay and vision/avoidance logging profiles @@ -164,6 +154,8 @@ param set-default SDLOG_DIRS_MAX 7 param set-default TRIG_INTERFACE 3 +param set-default SYS_FAILURE_EN 1 + # Adapt timeout parameters if simulation runs faster or slower than realtime. if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc) @@ -205,49 +197,46 @@ fi . "$autostart_file" -# -# If autoconfig parameter was set, reset it and save parameters. -# -if [ $AUTOCNF = yes ] -then - param set SYS_AUTOCONFIG 0 -fi - # Simulator IMU data provided at 250 Hz param set IMU_INTEG_RATE 250 +#user defined params for instances can be in PATH +. px4-rc.params + dataman start +# start sih in sih_sim mode, otherwise simulator module +if [ "$SIM_MODE" = "sihsim" ]; then + sih start # only start the simulator if not in replay mode, as both control the lockstep time -if ! replay tryapplyparams +elif ! replay tryapplyparams then - # Check if PX4_SIM_HOSTNAME environment variable is empty - # If empty check if PX4_SIM_HOST_ADDR environment variable is empty - # If both are empty use localhost for simulator - if [ -z "${PX4_SIM_HOSTNAME}" ]; then - if [ -z "${PX4_SIM_HOST_ADDR}" ]; then - echo "PX4 SIM HOST: localhost" - simulator start -c $simulator_tcp_port - else - echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" - simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port - fi - else - echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" - simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port - fi + . px4-rc.simulator fi load_mon start battery_simulator start tone_alarm start rc_update start +manual_control start sensors start commander start + +# Configure vehicle type specific parameters. +# Note: rc.vehicle_setup is the entry point for rc.interface, +# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. +# +. ${R}etc/init.d/rc.vehicle_setup + navigator start +# Try to start the micrortps_client with UDP transport if module exists +if px4-micrortps_client status > /dev/null 2>&1 +then + . px4-rc.rtps +fi if param greater -s MNT_MODE_IN -1 then - vmount start + gimbal start fi if param greater -s TRIG_MODE 0 @@ -266,38 +255,8 @@ then gyro_calibration start fi -# Configure vehicle type specific parameters. -# Note: rc.vehicle_setup is the entry point for rc.interface, -# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. -# -. ${R}etc/init.d/rc.vehicle_setup - -if [ -e etc/init.d-posix/rc.mavlink_override ] -then - echo "Running non-default mavlink config rc.mavlink_override" - . ${R}etc/init.d-posix/rc.mavlink_override -else - # GCS link - mavlink start -x -u $udp_gcs_port_local -r 4000000 -f - mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local - mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local - mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local - mavlink stream -r 50 -s ATTITUDE -u $udp_gcs_port_local - mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_gcs_port_local - mavlink stream -r 50 -s ATTITUDE_TARGET -u $udp_gcs_port_local - mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local - mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local - mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local - - # API/Offboard link - mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote - - # Onboard link to camera - 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 -fi +#user defined mavlink streams for instances can be in PATH +. px4-rc.mavlink # execute autostart post script if any [ -e "$autostart_file".post ] && . "$autostart_file".post diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index ef190544ae0f..248350cd4ec8 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -42,7 +42,6 @@ px4_add_romfs_files( rc.fw_apps rc.fw_defaults rc.interface - rc.io rc.logging rc.mc_apps rc.mc_defaults @@ -56,5 +55,4 @@ px4_add_romfs_files( rc.vehicle_setup rc.vtol_apps rc.vtol_defaults - rc.ctrlalloc ) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil index 0b7aa78871a8..c96535bea4ed 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil @@ -13,12 +13,12 @@ # @output MAIN6 gear # # @maintainer Lorenz Meier +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.fw_defaults - -param set-default BAT_N_CELLS 3 +param set-default BAT1_N_CELLS 3 param set-default FW_AIRSPD_MAX 20 param set-default FW_AIRSPD_MIN 12 @@ -29,24 +29,40 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HHDIST 15 param set-default FW_LND_HVIRT 13 param set-default FW_LND_TLALT 5 param set-default FW_THR_LND_MAX 0 param set-default FW_PR_FF 0.35 -param set-default FW_PR_I 0.1 -param set-default FW_PR_IMAX 0.4 param set-default FW_PR_P 0.2 param set-default FW_RR_FF 0.6 -param set-default FW_RR_I 0.1 -param set-default FW_RR_IMAX 0.2 param set-default FW_RR_P 0.3 param set-default RWTO_TKOFF 1 +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.0 +param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS2_TRQ_Y 1.0 +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 param set SYS_HITL 1 +param set UAVCAN_ENABLE 0 + # disable some checks to allow to fly # - with usb param set-default CBRK_USB_CHK 197848 diff --git a/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery index a33707b682da..0b01898f4c3a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery @@ -25,19 +25,12 @@ . ${R}etc/init.d/rc.mc_defaults - -param set-default MC_ROLL_P 6.5 param set-default MC_ROLLRATE_P 0.1 param set-default MC_ROLLRATE_I 0.05 param set-default MC_ROLLRATE_D 0.0017 -param set-default MC_PITCH_P 6.5 param set-default MC_PITCHRATE_P 0.14 param set-default MC_PITCHRATE_I 0.1 param set-default MC_PITCHRATE_D 0.0025 -param set-default MC_YAW_P 2.8 param set-default MC_YAWRATE_P 0.28 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -set MIXER quad_w -set PWM_OUT 1234 +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris index ea593e825b8c..cbc9a768e3ee 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris @@ -23,23 +23,18 @@ . ${R}etc/init.d/rc.mc_defaults - # TODO tune roll/pitch separately param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLLRATE_I 0.05 param set-default MC_ROLLRATE_D 0.004 param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCHRATE_I 0.05 param set-default MC_PITCHRATE_D 0.004 param set-default MC_YAW_P 2.5 param set-default MC_YAWRATE_P 0.25 param set-default MC_YAWRATE_I 0.25 -param set-default MC_YAWRATE_D 0 -param set-default BAT_V_DIV 12.27559 -param set-default BAT_A_PER_V 15.39103 -set MIXER quad_w +param set-default BAT1_V_DIV 12.27559 +param set-default BAT1_A_PER_V 15.391030303103 -set PWM_OUT 1234 +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d/airframes/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/airframes/10017_steadidrone_qu4d deleted file mode 100644 index 1d7e3e268cc5..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/10017_steadidrone_qu4d +++ /dev/null @@ -1,45 +0,0 @@ -#!/bin/sh -# -# @name Steadidrone QU4D -# -# @type Quadrotor Wide -# @class Copter -# -# @output MAIN1 motor 1 -# @output MAIN2 motor 2 -# @output MAIN3 motor 3 -# @output MAIN4 motor 4 -# @output MAIN5 feed-through of RC AUX1 channel -# @output MAIN6 feed-through of RC AUX2 channel -# -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# @output AUX4 feed-through of RC FLAPS channel -# -# @maintainer Lorenz Meier -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default BAT_N_CELLS 4 - -param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.13 -param set-default MC_ROLLRATE_I 0.05 -param set-default MC_ROLLRATE_D 0.004 -param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.19 -param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.004 -param set-default MC_YAW_P 4 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -set MIXER quad_w - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance index 05b39e571ebd..bbf00f86bc49 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance @@ -25,26 +25,19 @@ . ${R}etc/init.d/rc.mc_defaults - -param set-default BAT_N_CELLS 6 -param set-default BAT_V_EMPTY 3.5 +param set-default BAT1_N_CELLS 6 +param set-default BAT1_V_EMPTY 3.5 param set-default MC_ROLL_P 7 param set-default MC_ROLLRATE_P 0.08 param set-default MC_ROLLRATE_I 0.02 -param set-default MC_ROLLRATE_D 0.003 param set-default MC_PITCH_P 7 param set-default MC_PITCHRATE_P 0.13 param set-default MC_PITCHRATE_I 0.02 param set-default MC_PITCHRATE_D 0.005 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 param set-default MPC_XY_VEL_MAX 2 param set-default PWM_MAIN_MIN 1080 -set MIXER quad_w -set PWM_OUT 1234 +set MIXER quad_w diff --git a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil index 3cf60a0f9808..f3c27312f611 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil @@ -6,15 +6,34 @@ # @class Copter # # @maintainer Lorenz Meier +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.mc_defaults set MIXER quad_x -set PWM_OUT 1234 param set SYS_HITL 1 +param set UAVCAN_ENABLE 0 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 + +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 + # disable some checks to allow to fly # - with usb param set-default CBRK_USB_CHK 197848 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 0eb7b40ff224..3b9d7f7aa34f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -9,19 +9,18 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults -param set-default BAT_N_CELLS 3 +param set-default BAT1_N_CELLS 3 param set-default COM_RC_IN_MODE 1 -param set-default EKF2_AID_MASK 1 param set-default EKF2_ANGERR_INIT 0.01 param set-default EKF2_GBIAS_INIT 0.01 -param set-default EKF2_HGT_MODE 0 param set-default EKF2_MAG_TYPE 1 param set-default FW_AIRSPD_MAX 25 @@ -50,22 +49,53 @@ param set-default MPC_Z_VEL_MAX_DN 1.5 param set-default NAV_ACC_RAD 5 param set-default NAV_DLL_ACT 2 -param set-default NAV_LOITER_RAD 80 param set-default RTL_DESCEND_ALT 10 param set-default RTL_RETURN_ALT 30 param set-default SDLOG_DIRS_MAX 7 -param set-default SYS_RESTART_TYPE 2 param set-default VT_F_TRANS_THR 0.75 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_TYPE 2 +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 + +param set-default CA_SV_CS_COUNT 3 +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.0 +param set-default CA_SV_CS2_TYPE 3 + +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 105 +param set-default HIL_ACT_FUNC6 201 +param set-default HIL_ACT_FUNC7 202 +param set-default HIL_ACT_FUNC8 203 + param set SYS_HITL 1 +param set UAVCAN_ENABLE 0 + # disable some checks to allow to fly # - with usb param set-default CBRK_USB_CHK 197848 @@ -75,7 +105,7 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default COM_PREARM_MODE 0 param set-default CBRK_IO_SAFETY 22027 -set MAV_TYPE 22 +param set-default MAV_TYPE 22 set MIXER standard_vtol_hitl diff --git a/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox index b36f11ef3440..0b0495ffd3ae 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox @@ -19,6 +19,7 @@ # @maintainer Lorenz Meier # # @board bitcraze_crazyflie exclude +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil index 9011987384db..61400342abb6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil @@ -15,6 +15,25 @@ set MIXER quad_x set PWM_OUT 1234 +param set UAVCAN_ENABLE 0 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 + +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 + # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set SYS_HITL 2 @@ -26,3 +45,5 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default COM_PREARM_MODE 0 param set-default CBRK_IO_SAFETY 22027 + +param set SIH_VEHICLE_TYPE 0 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 new file mode 100644 index 000000000000..63e3ce3e308c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -0,0 +1,63 @@ +#!/bin/sh +# +# @name SIH plane AERT +# +# @type Simulation +# @class Plane +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.fw_defaults + +set MIXER AERT +set PWM_OUT 1234 + +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.0 +param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS2_TRQ_Y 1.0 +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 + +# 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: +# - with usb +param set-default CBRK_USB_CHK 197848 +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set-default BAT_N_CELLS 3 + +param set SIH_T_MAX 6.0 +param set SIH_MASS 0.3 +param set SIH_IXX 0.00402 +param set SIH_IYY 0.0144 +param set SIH_IZZ 0.0177 +param set SIH_IXZ 0.00046 +param set SIH_KDV 0.2 + +param set SIH_VEHICLE_TYPE 1 # sih as fixed wing +param set RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched) diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil new file mode 100644 index 000000000000..a9046c6aff4e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -0,0 +1,81 @@ +#!/bin/sh +# +# @name SIH Tailsitter Duo +# +# @type Simulation +# @class VTOL +# +# @output MAIN1 motor right +# @output MAIN2 motor left +# @output MAIN5 elevon right +# @output MAIN6 elevon left +# +# @maintainer Romain Chiappinelli +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +param set UAVCAN_ENABLE 0 +param set-default VT_ELEV_MC_LOCK 0 +param set-default VT_MOT_COUNT 2 +param set-default VT_TYPE 0 +param set-default VT_FW_DIFTHR_EN 1 +param set-default VT_FW_DIFTHR_SC 0.3 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default CA_AIRFRAME 4 +param set-default CA_ROTOR_COUNT 2 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TRQ_P 0.3 +param set-default CA_SV_CS0_TRQ_Y 0.3 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS1_TRQ_P 0.3 +param set-default CA_SV_CS1_TRQ_Y -0.3 +param set-default CA_SV_CS1_TYPE 6 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC5 202 +param set-default HIL_ACT_FUNC6 201 +param set-default HIL_ACT_REV 32 + +param set-default MAV_TYPE 19 + +set MIXER vtol_tailsitter_duo_sat + +set PWM_OUT 1234 + +# 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: +# - with usb +param set-default CBRK_USB_CHK 197848 +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default COM_PREARM_MODE 0 +param set-default CBRK_IO_SAFETY 22027 + +param set-default BAT_N_CELLS 3 + +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.0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as tailsitter +param set SIH_VEHICLE_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/12002_steadidrone_mavrik b/ROMFS/px4fmu_common/init.d/airframes/12002_steadidrone_mavrik deleted file mode 100644 index e91e70e8d686..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/12002_steadidrone_mavrik +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/sh -# -# @name Steadidrone MAVRIK -# -# @type Octo Coax Wide -# @class Copter -# -# @output MAIN1 motor 1 -# @output MAIN2 motor 2 -# @output MAIN3 motor 3 -# @output MAIN4 motor 4 -# @output MAIN5 motor 5 -# @output MAIN6 motor 6 -# @output MAIN7 motor 7 -# @output MAIN8 motor 8 -# -# @maintainer Simon Wilks -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default MC_PITCH_P 4 -param set-default MC_PITCHRATE_P 0.24 -param set-default MC_PITCHRATE_I 0.09 -param set-default MC_PITCHRATE_D 0.013 -param set-default MC_PITCHRATE_MAX 180 - -param set-default MC_ROLL_P 4 -param set-default MC_ROLLRATE_P 0.16 -param set-default MC_ROLLRATE_I 0.07 -param set-default MC_ROLLRATE_D 0.009 -param set-default MC_ROLLRATE_MAX 180 - -param set-default MC_YAW_P 3 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 - -param set-default MPC_HOLD_MAX_XY 0.25 -param set-default MPC_THR_MIN 0.15 -param set-default MPC_Z_VEL_MAX_DN 2 - -param set-default BAT_N_CELLS 4 -set MIXER octo_cox_w - -set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard index 6029d176ffbe..4bdbb3001595 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard +++ b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard @@ -19,18 +19,43 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_ROTOR4_PX 0.2 +param set-default CA_SV_CS_COUNT 4 +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.0 +param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TYPE 4 param set-default PWM_AUX_DIS5 950 -param set-default PWM_MAIN_RATE 400 param set-default VT_TYPE 2 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 -set MAV_TYPE 22 + +param set-default MAV_TYPE 22 set MIXER quad_x set MIXER_AUX vtol_AAERT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol index 30b8c7111fdf..85e55bbe9dda 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol @@ -14,36 +14,29 @@ # # @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 19 param set-default MC_ROLL_P 6 param set-default MC_ROLLRATE_P 0.12 param set-default MC_ROLLRATE_I 0.002 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_ROLLRATE_FF 0 param set-default MC_PITCH_P 4.5 param set-default MC_PITCHRATE_P 0.3 param set-default MC_PITCHRATE_I 0.002 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_PITCHRATE_FF 0 param set-default MC_YAW_P 3.8 param set-default MC_YAWRATE_P 0.22 param set-default MC_YAWRATE_I 0.02 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_MOT_ID 12 param set-default VT_TYPE 0 -set MAV_TYPE 19 set MIXER vtol_tailsitter_duo diff --git a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 index 8660126b1230..583a045696ad 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 @@ -23,24 +23,19 @@ . ${R}etc/init.d/rc.vtol_defaults +param set-default MAV_TYPE 21 param set-default MC_ROLL_P 7 param set-default MC_ROLLRATE_P 0.19 param set-default MC_ROLLRATE_I 0.002 param set-default MC_ROLLRATE_D 0.005 -param set-default MC_ROLLRATE_FF 0 param set-default MC_PITCH_P 7 param set-default MC_PITCHRATE_P 0.14 param set-default MC_PITCHRATE_I 0.002 param set-default MC_PITCHRATE_D 0.004 -param set-default MC_PITCHRATE_FF 0 param set-default MC_YAW_P 4 param set-default MC_YAWRATE_P 0.22 param set-default MC_YAWRATE_I 0.02 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 - -param set-default PWM_MAIN_RATE 400 param set-default VT_FW_MOT_OFFID 34 param set-default VT_IDLE_PWM_MC 1080 @@ -51,7 +46,6 @@ param set-default VT_TILT_TRANS 0.5 param set-default VT_TILT_FW 0.9 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 1 -set MAV_TYPE 21 set MIXER firefly6 set MIXER_AUX firefly6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter index ea700ce08482..e88f711105b2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/airframes/13003_quad_tailsitter @@ -13,15 +13,33 @@ . ${R}etc/init.d/rc.vtol_defaults +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.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_Y 0.5 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_Y -0.5 +param set-default CA_SV_CS1_TYPE 6 param set-default PWM_MAIN_MAX 2000 -param set-default PWM_MAIN_RATE 400 param set-default VT_MOT_ID 1234 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_TYPE 0 -param set-default VT_ELEV_MC_LOCK 1 -set MAV_TYPE 20 set MIXER quad_x_vtol diff --git a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter index fdf9a3daf846..d162e824759b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter +++ b/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter @@ -7,8 +7,8 @@ # # @output MAIN1 motor 1 # @output MAIN2 motor 2 -# @output MAIN3 motor 4 -# @output MAIN4 motor 5 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 # @output MAIN5 elevon left # @output MAIN6 elevon right # @output MAIN7 canard surface @@ -22,14 +22,12 @@ . ${R}etc/init.d/rc.vtol_defaults +param set-default MAV_TYPE 20 param set-default PWM_MAIN_MAX 2000 -param set-default PWM_MAIN_RATE 400 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_TYPE 0 -param set-default VT_ELEV_MC_LOCK 1 -set MAV_TYPE 20 set MIXER quad_+_vtol diff --git a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad index 03151f142614..8949f18f20ad 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad +++ b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad @@ -19,42 +19,30 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults - param set-default PWM_AUX_DIS5 950 -param set-default PWM_MAIN_RATE 400 - param set-default MC_ROLL_P 6 param set-default MC_ROLLRATE_P 0.17 param set-default MC_ROLLRATE_I 0.002 param set-default MC_ROLLRATE_D 0.004 -param set-default MC_ROLLRATE_FF 0 param set-default MC_PITCH_P 6 param set-default MC_PITCHRATE_P 0.19 param set-default MC_PITCHRATE_I 0.002 param set-default MC_PITCHRATE_D 0.004 -param set-default MC_PITCHRATE_FF 0 -param set-default MC_YAW_P 2.8 param set-default MC_YAWRATE_P 0.22 param set-default MC_YAWRATE_I 0.02 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 param set-default MC_YAWRATE_MAX 40 param set-default MPC_YAWRAUTO_MAX 40 -param set-default FW_PR_FF 0.5 param set-default FW_PR_I 0.02 -param set-default FW_PR_IMAX 0.4 -param set-default FW_PR_P 0.08 param set-default FW_RR_FF 0.6 param set-default FW_RR_I 0.01 -param set-default FW_RR_IMAX 0.2 -param set-default FW_RR_P 0.05 param set-default FW_THR_CRUISE 0.75 param set-default VT_ARSP_BLEND 6 @@ -64,7 +52,6 @@ param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_TYPE 2 -set MAV_TYPE 22 set MIXER quad_x set MIXER_AUX vtol_AAERT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta b/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta index b50ab35cbe03..220747513503 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta +++ b/ROMFS/px4fmu_common/init.d/airframes/13006_vtol_standard_delta @@ -17,26 +17,16 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults -param set-default MC_ROLL_P 6.5 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLLRATE_I 0.01 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_ROLLRATE_FF 0 -param set-default MC_PITCH_P 6.5 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCHRATE_I 0.01 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_PITCHRATE_FF 0 param set-default MC_YAW_P 3.5 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 + param set-default MC_YAWRATE_MAX 50 param set-default MPC_XY_P 0.8 @@ -45,14 +35,12 @@ param set-default MPC_ACC_HOR_MAX 2 param set-default MPC_YAWRAUTO_MAX 20 param set-default PWM_AUX_DIS3 950 -param set-default PWM_MAIN_RATE 400 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_F_TRANS_THR 0.75 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_TYPE 2 -set MAV_TYPE 22 set MIXER quad_x set MIXER_AUX vtol_delta diff --git a/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad b/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad index 62ad1e71a30d..7671ec1cf503 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad +++ b/ROMFS/px4fmu_common/init.d/airframes/13007_vtol_AAVVT_quad @@ -9,39 +9,30 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLLRATE_I 0.002 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_ROLLRATE_FF 0 param set-default MC_PITCH_P 7 param set-default MC_PITCHRATE_P 0.12 param set-default MC_PITCHRATE_I 0.002 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_PITCHRATE_FF 0 -param set-default MC_YAW_P 2.8 param set-default MC_YAWRATE_P 0.22 param set-default MC_YAWRATE_I 0.02 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 param set-default MC_YAWRATE_MAX 40 param set-default MPC_YAWRAUTO_MAX 40 param set-default PWM_AUX_DIS5 950 -param set-default PWM_MAIN_RATE 400 param set-default VT_F_TRANS_THR 0.75 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_TYPE 2 -set MAV_TYPE 22 set MIXER quad_x set MIXER_AUX vtol_AAVVT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger index a04b06f68aa8..94e2a1a9130b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger +++ b/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger @@ -15,50 +15,37 @@ param set-default FW_THR_CRUISE 65 -param set-default FW_PR_P 0.08 -param set-default FW_PR_FF 0.5 -param set-default FW_RR_P 0.05 param set-default FW_RR_FF 0.6 param set-default MIS_YAW_TMT 10 param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLLRATE_I 0.1 param set-default MC_ROLLRATE_D 0.004 -param set-default MC_ROLLRATE_FF 0 param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCHRATE_I 0.1 param set-default MC_PITCHRATE_D 0.004 -param set-default MC_PITCHRATE_FF 0 param set-default MC_YAW_P 3.5 param set-default MC_YAWRATE_P 0.6 param set-default MC_YAWRATE_I 0.04 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 param set-default MC_YAWRATE_MAX 40 param set-default MPC_ACC_HOR_MAX 2 param set-default MPC_Z_VEL_MAX_DN 1.5 -param set-default MPC_TKO_SPEED 1.5 param set-default MPC_LAND_SPEED 0.8 param set-default MPC_YAWRAUTO_MAX 40 param set-default PWM_AUX_DIS5 950 param set-default PWM_AUX_REV1 1 param set-default PWM_AUX_REV2 1 -param set-default PWM_MAIN_RATE 400 param set-default VT_ARSP_TRANS 15 param set-default VT_ARSP_BLEND 8 -param set-default VT_B_TRANS_DUR 4 param set-default VT_F_TRANS_THR 0.75 param set-default VT_IDLE_PWM_MC 1080 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_TYPE 2 -set MAV_TYPE 22 set MIXER quad_x set MIXER_AUX vtol_AAERT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger index 47f0c98f41dd..312ffd3e5b63 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger +++ b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger @@ -9,11 +9,11 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults - param set-default FW_AIRSPD_MAX 22 param set-default FW_AIRSPD_MIN 14 param set-default FW_AIRSPD_TRIM 16 @@ -68,10 +68,7 @@ param set-default PWM_AUX_REV4 1 param set-default PWM_AUX_DIS5 950 -param set-default PWM_MAIN_RATE 400 - param set-default VT_ARSP_TRANS 15 -param set-default VT_B_TRANS_DUR 4 param set-default VT_F_TRANS_THR 0.6 param set-default VT_IDLE_PWM_MC 1180 param set-default VT_MOT_ID 1234 @@ -80,8 +77,6 @@ param set-default VT_TRANS_MIN_TM 5 param set-default VT_TRANS_TIMEOUT 30 param set-default VT_TYPE 2 -set MAV_TYPE 22 - set MIXER quad_x set MIXER_AUX vtol_AAERT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13010_claire b/ROMFS/px4fmu_common/init.d/airframes/13010_claire deleted file mode 100644 index c9ef139257ad..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/13010_claire +++ /dev/null @@ -1,38 +0,0 @@ -#!/bin/sh -# -# @name CruiseAder Claire -# -# @type VTOL Tiltrotor -# @class VTOL -# -# @maintainer Samay Siga -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.vtol_defaults - - -param set-default PWM_AUX_DISARMED 1000 -param set-default PWM_AUX_MAX 2000 -param set-default PWM_AUX_MIN 1000 -param set-default PWM_AUX_RATE 50 - -param set-default PWM_MAIN_MAX 2000 -param set-default PWM_MAIN_RATE 400 - -param set-default VT_MOT_ID 1234 -param set-default VT_FW_MOT_OFFID 13 -param set-default VT_IDLE_PWM_MC 1080 -param set-default VT_TILT_FW 0.9 -param set-default VT_TILT_MC 0.08 -param set-default VT_TILT_TRANS 0.5 -param set-default VT_ELEV_MC_LOCK 0 -param set-default VT_TYPE 1 -set MAV_TYPE 21 - -set MIXER claire -set MIXER_AUX claire - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence index 8c833c0c2809..7cc6fc4d8550 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence +++ b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence @@ -22,6 +22,7 @@ . ${R}etc/init.d/rc.vtol_defaults +param set-default MAV_TYPE 21 param set-default CBRK_AIRSPD_CHK 162128 @@ -32,22 +33,14 @@ param set-default FW_MAN_R_MAX 50 param set-default FW_ACRO_X_MAX 270 param set-default FW_ACRO_Y_MAX 270 param set-default FW_ACRO_Z_MAX 180 -param set-default FW_PR_FF 0.5 -param set-default FW_PR_P 0.08 param set-default FW_PSP_OFF 5 param set-default FW_P_LIM_MAX 30 param set-default FW_P_LIM_MIN -30 -param set-default FW_P_RMAX_NEG 60 -param set-default FW_P_RMAX_POS 60 param set-default FW_RR_FF 0.33 param set-default FW_RR_P 0.11 -param set-default FW_YR_FF 0.3 -param set-default FW_YR_P 0.05 -param set-default MC_PITCHRATE_D 0.003 param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCH_P 6 -param set-default MC_ROLLRATE_D 0.003 param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLL_P 6 param set-default MC_YAWRATE_MAX 120 @@ -57,9 +50,6 @@ param set-default MC_YAW_P 2.5 param set-default MC_YAWRATE_P 0.3 param set-default MPC_LAND_SPEED 1.2 param set-default MPC_TKO_SPEED 2.5 -param set-default MPC_Z_VEL_MAX_UP 3 - -param set-default PWM_MAIN_RATE 400 param set-default SENS_BOARD_ROT 8 @@ -69,18 +59,19 @@ param set-default VT_F_TR_OL_TM 4 param set-default VT_FW_DIFTHR_EN 1 param set-default VT_FW_DIFTHR_SC 0.17 param set-default VT_FW_MOT_OFFID 3 -param set-default VT_FW_PERM_STAB 0 param set-default VT_IDLE_PWM_MC 1200 param set-default VT_MOT_ID 123 -param set-default VT_TILT_FW 1 -param set-default VT_TILT_MC 0 param set-default VT_TILT_TRANS 0.45 param set-default VT_TRANS_MIN_TM 1.2 param set-default VT_TRANS_P2_DUR 1.3 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 1 -set MAV_TYPE 21 set MIXER vtol_convergence -set PWM_OUT 1234 +if ! ver hwcmp MATEK_H743 +then + set PWM_OUT 1234 +else + set PWM_OUT 3456 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 01a91748d9f0..394227a33f51 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -22,10 +22,11 @@ . ${R}etc/init.d/rc.vtol_defaults +param set-default MAV_TYPE 22 -param set-default BAT_CAPACITY 23000 -param set-default BAT_N_CELLS 4 -param set-default BAT_R_INTERNAL 0.0025 +param set-default BAT1_CAPACITY 23000 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_R_INTERNAL 0.0025 param set-default CBRK_AIRSPD_CHK 162128 param set-default CBRK_IO_SAFETY 22027 @@ -65,27 +66,16 @@ param set-default FW_BAT_SCALE_EN 1 param set-default FW_THR_ALT_SCL 2.7 param set-default FW_T_RLL2THR 20 -param set-default LNDMC_ALT_MAX 9999 param set-default LNDMC_XY_VEL_MAX 1 param set-default LNDMC_Z_VEL_MAX 0.7 -param set-default MC_ROLL_P 6.5 param set-default MC_ROLLRATE_P 0.16 param set-default MC_ROLLRATE_I 0.01 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_ROLLRATE_FF 0 param set-default MC_ROLLRATE_MAX 80 -param set-default MC_PITCH_P 6.5 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_PITCHRATE_FF 0 param set-default MC_PITCHRATE_MAX 80 param set-default MC_YAW_P 3.5 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 + param set-default MC_YAWRATE_MAX 20 param set-default MC_AIRMODE 1 @@ -109,17 +99,15 @@ 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_DLL_ACT 0 param set-default NAV_LOITER_RAD 100 -param set-default PWM_AUX_DISARMED 950 +param set-default PWM_AUX_DISARM 950 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 PWM_MAIN_RATE 400 param set-default SENS_BOARD_ROT 18 @@ -133,24 +121,20 @@ param set-default VT_TYPE 2 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 1234 param set-default VT_F_TRANS_THR 1 -param set-default VT_DWN_PITCH_MAX 8 +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_B_TRANS_DUR 8 -param set-default VT_WV_LND_EN 1 -param set-default VT_WV_LTR_EN 1 param set-default VT_FWD_THRUST_SC 4 param set-default VT_F_TRANS_DUR 1 param set-default VT_IDLE_PWM_MC 1025 param set-default VT_B_REV_OUT 0.5 param set-default VT_B_TRANS_THR 0.7 -param set-default VT_FW_PERM_STAB 1 param set-default VT_TRANS_TIMEOUT 22 param set-default VT_F_TRANS_RAMP 4 param set-default COM_RC_OVERRIDE 0 -set MAV_TYPE 22 set MIXER deltaquad set MIXER_AUX pass diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 12d48b0ef33f..14578b70f23c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -18,26 +18,24 @@ # # @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 BAT_N_CELLS 6 +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_L1_R_SLEW_MAX 40 -param set-default FW_LND_EARLYCFG 1 param set-default FW_MAN_P_MAX 30 -param set-default FW_PR_I 0.1 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_I 0.1 param set-default FW_RR_P 0.08 param set-default FW_R_LIM 45 param set-default FW_R_RMAX 50 @@ -47,22 +45,15 @@ param set-default FW_THR_SLEW_MAX 0.6 param set-default FW_T_HRATE_FF 0 param set-default FW_T_SINK_MAX 15 param set-default FW_T_SINK_MIN 3 -param set-default FW_YR_I 0.1 param set-default FW_YR_P 0.15 -param set-default IMU_GYRO_CUTOFF 40 - param set-default IMU_DGYRO_CUTOFF 15 -param set-default MC_PITCHRATE_I 0.2 param set-default MC_PITCHRATE_MAX 60 -param set-default MC_ROLLRATE_I 0.2 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 MIS_TAKEOFF_ALT 30 - param set-default MPC_ACC_DOWN_MAX 2 param set-default MPC_ACC_HOR_MAX 2 param set-default MPC_ACC_UP_MAX 3 @@ -71,8 +62,7 @@ param set-default MPC_JERK_AUTO 4 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_POS_MODE 3 -param set-default MPC_SPOOLUP_TIME 1.5 +param set-default COM_SPOOLUP_TIME 1.5 param set-default MPC_THR_HOVER 0.45 param set-default MPC_TILTMAX_AIR 25 param set-default MPC_TKO_RAMP_T 1.8 @@ -106,10 +96,9 @@ param set-default VT_PSHER_RMP_DT 2 param set-default VT_TRANS_MIN_TM 4 param set-default VT_TYPE 2 -set MAV_TYPE 22 - set MIXER babyshark set MIXER_AUX pass +# Mark outputs for the alternate rate +# or D-Shot set PWM_OUT 5678 -set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor index e6595c74ff93..d62b42ee71b7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor +++ b/ROMFS/px4fmu_common/init.d/airframes/13030_generic_vtol_quad_tiltrotor @@ -22,18 +22,42 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults - -param set-default PWM_MAIN_RATE 400 +param set-default MAV_TYPE 21 param set-default VT_IDLE_PWM_MC 1100 param set-default VT_TYPE 1 param set-default VT_MOT_ID 1234 param set-default VT_FW_MOT_OFFID 24 -set MAV_TYPE 21 + +param set-default CA_AIRFRAME 3 +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR0_TILT 1 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR1_TILT 2 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR2_TILT 3 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR3_TILT 4 +param set-default CA_SV_CS_COUNT 4 +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.0 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TYPE 4 +param set-default CA_SV_TL_COUNT 4 set MIXER quad_x set MIXER_AUX vtol_TTTTAAER diff --git a/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo b/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo index 6daf76613662..c56a10d69250 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo +++ b/ROMFS/px4fmu_common/init.d/airframes/13050_generic_vtol_octo @@ -21,18 +21,16 @@ # # @board px4_fmu-v2 exclude # @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude # . ${R}etc/init.d/rc.vtol_defaults - param set-default PWM_AUX_DIS5 950 -param set-default PWM_MAIN_RATE 400 param set-default VT_TYPE 2 param set-default VT_MOT_ID 12345678 param set-default VT_FW_MOT_OFFID 12345678 -set MAV_TYPE 22 set MIXER octo_cox set MIXER_AUX vtol_AAERT diff --git a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter index 23ea7fb2c935..d235153980b3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter +++ b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter @@ -14,17 +14,31 @@ # # @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 19 param set-default VT_ELEV_MC_LOCK 0 param set-default VT_MOT_COUNT 2 param set-default VT_TYPE 0 -param set-default MAV_TYPE 19 -set MAV_TYPE 19 +param set-default CA_AIRFRAME 4 +param set-default CA_ROTOR_COUNT 2 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_Y 0.5 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_Y -0.5 +param set-default CA_SV_CS1_TYPE 6 + set MIXER vtol_tailsitter_duo set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/14001_tri_y_yaw+ b/ROMFS/px4fmu_common/init.d/airframes/14001_tri_y_yaw+ index eb02d83734e7..e12393eb6eea 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/14001_tri_y_yaw+ +++ b/ROMFS/px4fmu_common/init.d/airframes/14001_tri_y_yaw+ @@ -13,10 +13,12 @@ # @maintainer Trent Lukaczyk # # @board bitcraze_crazyflie exclude +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.mc_defaults -set MIXER tri_y_yaw+ +# MAV_TYPE_TRICOPTER 15 +param set-default MAV_TYPE 15 -set PWM_OUT 1234 +set MIXER tri_y_yaw+ diff --git a/ROMFS/px4fmu_common/init.d/airframes/14002_tri_y_yaw- b/ROMFS/px4fmu_common/init.d/airframes/14002_tri_y_yaw- index ad36ed08c00a..425728199cfb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/14002_tri_y_yaw- +++ b/ROMFS/px4fmu_common/init.d/airframes/14002_tri_y_yaw- @@ -13,10 +13,12 @@ # @maintainer Trent Lukaczyk # # @board bitcraze_crazyflie exclude +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.mc_defaults -set MIXER tri_y_yaw- +# MAV_TYPE_TRICOPTER 15 +param set-default MAV_TYPE 15 -set PWM_OUT 1234 +set MIXER tri_y_yaw- diff --git a/ROMFS/px4fmu_common/init.d/airframes/15001_coax_heli b/ROMFS/px4fmu_common/init.d/airframes/15001_coax_heli index 3113c8cc4154..a8a65b733320 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/15001_coax_heli +++ b/ROMFS/px4fmu_common/init.d/airframes/15001_coax_heli @@ -17,38 +17,32 @@ # . ${R}etc/init.d/rc.mc_defaults -set MIXER coax +# MAV_TYPE_COAXIAL 3 +param set-default MAV_TYPE 3 -param set-default MC_ROLL_P 6.5 param set-default MC_ROLLRATE_P 0.17 param set-default MC_ROLLRATE_I 0.05 param set-default MC_ROLLRATE_D 0.005 -param set-default MC_ROLLRATE_FF 0 -param set-default MC_PITCH_P 6.5 param set-default MC_PITCHRATE_P 0.17 param set-default MC_PITCHRATE_I 0.05 param set-default MC_PITCHRATE_D 0.005 -param set-default MC_PITCHRATE_FF 0 param set-default MC_YAW_P 2 param set-default MC_YAWRATE_P 0.1 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_FF 0 param set-default NAV_ACC_RAD 2 param set-default PWM_AUX_RATE 50 param set-default PWM_MAIN_DISARM 900 -param set-default PWM_MAIN_MIN 1075 -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_RATE 400 param set-default RTL_RETURN_ALT 30 param set-default RTL_DESCEND_ALT 10 + +set MIXER coax + # This is the gimbal pass mixer set MIXER_AUX pass # use PWM parameters for throttle channel -set PWM_AUX_OUT 1234 set PWM_OUT 34 diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter index 0367a747a893..ac1c5475c1f4 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter +++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter @@ -20,12 +20,11 @@ . ${R}etc/init.d/rc.mc_defaults # Configure as helicopter (number 4 defined in commander_helper.cpp) -set MAV_TYPE 4 +param set-default MAV_TYPE 4 set MIXER blade130 -#set PWM_OUT 1234 - +set PWM_OUT none param set-default ATT_BIAS_MAX 0 @@ -36,7 +35,6 @@ param set-default MC_ROLLRATE_P 0 param set-default MC_ROLLRATE_I 0 param set-default MC_ROLLRATE_D 0 param set-default MC_ROLLRATE_FF 0.15 -param set-default MC_PITCH_P 6.5 param set-default MC_PITCHRATE_P 0 param set-default MC_PITCHRATE_I 0 param set-default MC_PITCHRATE_D 0 @@ -44,7 +42,6 @@ param set-default MC_PITCHRATE_FF 0.15 param set-default MC_YAW_P 3 param set-default MC_YAWRATE_P 0.1 param set-default MC_YAWRATE_I 0 -param set-default MC_YAWRATE_D 0 param set-default MC_ROLLRATE_MAX 720 param set-default MC_PITCHRATE_MAX 720 param set-default MC_YAWRATE_MAX 400 diff --git a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 index 048f4bb1be1a..8c847cbb444c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 @@ -26,8 +26,8 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default BAT_CAPACITY 2500 -param set-default BAT_N_CELLS 3 +param set-default BAT1_CAPACITY 2500 +param set-default BAT1_N_CELLS 3 param set-default PWM_AUX_RATE 50 param set-default PWM_MAIN_RATE 50 @@ -46,5 +46,6 @@ param set-default FW_R_LIM 40 param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_P_RMAX_NEG 20 + set MIXER TF-AutoG2 set MIXER_AUX pass diff --git a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 index 975001bc1a4a..e677228cd835 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 +++ b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 @@ -21,8 +21,8 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default BAT_CAPACITY 3300 -param set-default BAT_N_CELLS 3 +param set-default BAT1_CAPACITY 3300 +param set-default BAT1_N_CELLS 3 param set-default PWM_AUX_RATE 50 param set-default PWM_MAIN_RATE 50 @@ -41,5 +41,6 @@ param set-default FW_R_LIM 40 param set-default FW_P_LIM_MAX 25 param set-default FW_P_LIM_MIN -5 param set-default FW_P_RMAX_NEG 20 + set MIXER TF-G2 set MIXER_AUX pass diff --git a/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 b/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 index e2ad00c80993..b62f1a983103 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 +++ b/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 @@ -15,7 +15,6 @@ . ${R}etc/init.d/rc.balloon_defaults - param set-default COM_PREARM_MODE 2 # always in prearm state param set-default CBRK_IO_SAFETY 22027 param set-default SDLOG_PROFILE 17 @@ -25,9 +24,5 @@ param set-default MAV_0_CONFIG 102 param set-default GPS_UBX_DYNMODEL 8 param set-default SER_TEL2_BAUD 9600 -param set-default SENS_BOARD_ROT 0 - param set MAV_TYPE 8 # MAV_TYPE_FREE_BALLOON - - set MIXER IO_pass set MIXER_AUX pass diff --git a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane index 827447250386..99e53f618bb2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane +++ b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane @@ -23,6 +23,18 @@ . ${R}etc/init.d/rc.fw_defaults +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_CS1_TRQ_R 0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS3_TRQ_Y 1.0 +param set-default CA_SV_CS3_TYPE 4 + param set-default PWM_AUX_RATE 50 param set-default PWM_MAIN_RATE 50 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2105_maja b/ROMFS/px4fmu_common/init.d/airframes/2105_maja index eda8c12d64b5..026a0c3043a6 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2105_maja +++ b/ROMFS/px4fmu_common/init.d/airframes/2105_maja @@ -25,10 +25,6 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default FW_AIRSPD_MIN 10 -param set-default FW_AIRSPD_TRIM 15 -param set-default FW_AIRSPD_MAX 20 - param set-default FW_MAN_P_MAX 55 param set-default FW_MAN_R_MAX 55 param set-default FW_R_LIM 55 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross index 7639f0b2d25a..47ed0c677b9d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross @@ -25,10 +25,6 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default FW_AIRSPD_MIN 10 -param set-default FW_AIRSPD_TRIM 15 -param set-default FW_AIRSPD_MAX 20 - param set-default FW_MAN_P_MAX 55 param set-default FW_MAN_R_MAX 55 param set-default FW_R_LIM 55 diff --git a/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon b/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon index 1f4825048f5e..8fbe544c5b1c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon +++ b/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon @@ -26,10 +26,6 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default FW_AIRSPD_MIN 10 -param set-default FW_AIRSPD_TRIM 15 -param set-default FW_AIRSPD_MAX 20 - param set-default FW_MAN_P_MAX 55 param set-default FW_MAN_R_MAX 55 param set-default FW_R_LIM 55 diff --git a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox index 37f1a8bc95ea..d2a4e75f48fb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox @@ -23,24 +23,23 @@ # @maintainer William Peale # # @board bitcraze_crazyflie exclude +# @board px4_fmu-v2 exclude # -set VEHICLE_TYPE mc - +. ${R}etc/init.d/rc.mc_defaults param set-default NAV_ACC_RAD 2 param set-default PWM_AUX_RATE 400 -param set-default PWM_AUX_DISARMED 900 +param set-default PWM_AUX_DISARM 900 param set-default PWM_AUX_MIN 1075 param set-default PWM_AUX_MAX 1950 -param set-default PWM_MAIN_MIN 1075 -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_RATE 400 param set-default RTL_DESCEND_ALT 10 param set-default RTL_RETURN_ALT 30 + set MIXER dodeca_top_cox set MIXER_AUX dodeca_bottom_cox diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index a040702c078e..3b15aeb9480f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -18,6 +18,5 @@ param set-default COM_PREARM_MODE 2 param set-default CBRK_IO_SAFETY 22027 - set MIXER cloudship set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/airframes/3030_io_camflyer deleted file mode 100644 index d902b960eb60..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/3030_io_camflyer +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/sh -# -# @name IO Camflyer -# -# @type Flying Wing -# @class Plane -# -# @output MAIN1 left aileron -# @output MAIN2 right aileron -# @output MAIN4 throttle -# -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# -# @maintainer Simon Wilks -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.fw_defaults - -param set-default FW_AIRSPD_MAX 15 -param set-default FW_AIRSPD_MIN 10 -param set-default FW_AIRSPD_TRIM 13 -param set-default FW_R_TC 0.3 -param set-default FW_P_TC 0.3 -param set-default FW_L1_DAMPING 0.74 -param set-default FW_L1_PERIOD 16 -param set-default FW_LND_ANG 15 -param set-default FW_LND_FLALT 5 -param set-default FW_LND_HHDIST 15 -param set-default FW_LND_HVIRT 13 -param set-default FW_LND_TLALT 5 -param set-default FW_THR_LND_MAX 0 -param set-default FW_PR_FF 0.35 -param set-default FW_PR_IMAX 0.4 -param set-default FW_PR_P 0.08 -param set-default FW_RR_FF 0.6 -param set-default FW_RR_IMAX 0.2 -param set-default FW_RR_P 0.04 - -param set-default PWM_MAIN_DISARM 1000 - - -set MIXER fw_generic_wing - -# Provide ESC a constant 1000 us pulse while disarmed -set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3031_phantom b/ROMFS/px4fmu_common/init.d/airframes/3031_phantom index 0b2a59476853..b31b2230ce29 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/airframes/3031_phantom @@ -24,20 +24,15 @@ . ${R}etc/init.d/rc.fw_defaults param set-default FW_AIRSPD_MIN 13 -param set-default FW_AIRSPD_TRIM 15 param set-default FW_AIRSPD_MAX 25 param set-default FW_R_TC 0.3 param set-default FW_P_TC 0.3 -param set-default FW_L1_DAMPING 0.75 param set-default FW_L1_PERIOD 15 param set-default FW_PR_FF 0.2 param set-default FW_PR_IMAX 0.2 param set-default FW_PR_P 0.03 param set-default FW_P_LIM_MAX 50 param set-default FW_P_LIM_MIN -50 -param set-default FW_P_ROLLFF 1 -param set-default FW_RR_FF 0.5 -param set-default FW_RR_IMAX 0.2 param set-default FW_RR_P 0.08 param set-default FW_R_LIM 50 param set-default FW_R_RMAX 50 @@ -48,8 +43,7 @@ param set-default PWM_MAIN_DISARM 1000 # the payload bay is pitched up about 7 degrees param set-default SENS_BOARD_Y_OFF 7 - -set MIXER phantom +set MIXER fw_generic_wing # Provide ESC a constant 1000 us pulse set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 index 8ea3dc7d3a28..f09bba392b19 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 @@ -30,7 +30,6 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HHDIST 15 param set-default FW_LND_HVIRT 13 param set-default FW_LND_TLALT 5 param set-default FW_THR_LND_MAX 0 @@ -38,5 +37,4 @@ param set-default FW_PR_FF 0.35 param set-default FW_RR_FF 0.6 param set-default FW_RR_P 0.04 - set MIXER fw_generic_wing diff --git a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing index ddb33dbfd54d..d9a18853a251 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing @@ -23,9 +23,8 @@ . ${R}etc/init.d/rc.fw_defaults -param set-default BAT_N_CELLS 2 +param set-default BAT1_N_CELLS 2 param set-default FW_AIRSPD_MAX 15 -param set-default FW_AIRSPD_MIN 10 param set-default FW_AIRSPD_TRIM 13 param set-default FW_R_TC 0.3 param set-default FW_P_TC 0.3 @@ -33,7 +32,6 @@ param set-default FW_L1_DAMPING 0.74 param set-default FW_L1_PERIOD 16 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 5 -param set-default FW_LND_HHDIST 15 param set-default FW_LND_HVIRT 13 param set-default FW_LND_TLALT 5 param set-default FW_THR_LND_MAX 0 @@ -43,12 +41,8 @@ param set-default FW_RR_P 0.04 param set-default PWM_MAIN_DISARM 1000 - -# Configure this as plane. -set MAV_TYPE 1 - # Set mixer. -set MIXER wingwing +set MIXER fw_generic_wing # Provide ESC a constant 1000 us pulse. set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3034_fx79 b/ROMFS/px4fmu_common/init.d/airframes/3034_fx79 deleted file mode 100644 index 192b53a6caa8..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/3034_fx79 +++ /dev/null @@ -1,31 +0,0 @@ -#!/bin/sh -# -# @name FX-79 Buffalo Flying Wing -# -# @type Flying Wing -# @class Plane -# -# @output MAIN1 right aileron -# @output MAIN2 left aileron -# @output MAIN4 throttle -# -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# -# @maintainer Simon Wilks -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.fw_defaults - -param set-default FW_AIRSPD_MAX 30 -param set-default FW_AIRSPD_MIN 13 -param set-default FW_AIRSPD_TRIM 15 - -param set-default NAV_LOITER_RAD 150 - - -set MIXER FX79 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3035_viper b/ROMFS/px4fmu_common/init.d/airframes/3035_viper deleted file mode 100644 index fc8998e1d888..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/3035_viper +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/sh -# -# @name Viper -# -# @type Flying Wing -# @class Plane -# @output MAIN1 left aileron -# @output MAIN2 right aileron -# @output MAIN4 throttle -# -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# -# @maintainer Simon Wilks -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.fw_defaults - -set MIXER Viper diff --git a/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon b/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon index 0c8f8fdd4410..b889f38db94f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon +++ b/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon @@ -27,17 +27,11 @@ param set-default FW_AIRSPD_MIN 15 param set-default FW_AIRSPD_TRIM 20 param set-default FW_AIRSPD_MAX 27 param set-default FW_ATT_TC 0.3 -param set-default FW_L1_DAMPING 0.75 -param set-default FW_L1_PERIOD 20 param set-default FW_PR_FF 0.35 param set-default FW_PR_IMAX 0.2 param set-default FW_PR_P 0.05 -param set-default FW_P_LIM_MAX 45 -param set-default FW_P_LIM_MIN -45 -param set-default FW_P_ROLLFF 1 param set-default FW_P_TC 0.3 param set-default FW_RR_FF 0.3 -param set-default FW_RR_IMAX 0.2 param set-default FW_RR_P 0.03 param set-default FW_R_LIM 40 param set-default FW_R_RMAX 50 @@ -49,8 +43,7 @@ param set-default PWM_MAIN_DISARM 1000 # the payload bay is pitched up about 7 degrees param set-default SENS_BOARD_Y_OFF 11.9 - -set MIXER phantom +set MIXER fw_generic_wing # Provide ESC a constant 1000 us pulse set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3037_parrot_disco_mod b/ROMFS/px4fmu_common/init.d/airframes/3037_parrot_disco_mod deleted file mode 100644 index 648b4db5ca37..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/3037_parrot_disco_mod +++ /dev/null @@ -1,89 +0,0 @@ -#!/bin/sh -# -# @name Modified Parrot Disco -# -# @url -# -# @type Flying Wing -# @class Plane -# -# @output MAIN1 left aileron -# @output MAIN2 right aileron -# @output MAIN4 throttle -# -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# -# @maintainer Jan Liphardt -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.fw_defaults - -#################################### -# Airspeed -#################################### -param set-default FW_AIRSPD_MIN 15 # = 29 knots -param set-default FW_AIRSPD_TRIM 20 # = 39 knots -param set-default FW_AIRSPD_MAX 27 # = 52 knots - -#################################### -# The Main L1 Controller -#################################### -param set-default FW_L1_PERIOD 20 #units of meters - -# Damping factor for L1 control (def = 0.75) -param set-default FW_L1_DAMPING 0.75 - -#################################### -# Pitch -#################################### - -# Basic limits (def = +/- 45 deg) -param set-default FW_P_LIM_MAX 45 -param set-default FW_P_LIM_MIN -45 - -# Time Constant (def = 0.4s) -param set-default FW_P_TC 0.4 - -# Pitch rate feed forward (def = 0.5 %/rad/sec) -param set-default FW_PR_FF 0.35 - -# Pitch rate integrator limit (def = 0.4) -param set-default FW_PR_IMAX 0.4 - -# Pitch rate proportional gain (def = 0.08 %/rad/sec) -param set-default FW_PR_P 0.08 - -#################################### -# Roll -#################################### - -# Basic limits (def = 50 deg) -param set-default FW_R_LIM 40 - -# Roll rate upper limit (def = 70 deg/s) -param set-default FW_R_RMAX 50 - -# Roll Time Constant (def = 0.4 s) -param set-default FW_R_TC 0.4 - -# Roll rate feed forward (def = 0.5 %/rad/sec) -param set-default FW_RR_FF 0.5 - -# Roll rate proportional Gain (def = 0.05 %/rad/sec) -param set-default FW_RR_P 0.05 - -# Roll Integrator Anti-Windup -param set-default FW_RR_IMAX 0.2 - -param set-default PWM_MAIN_DISARM 1000 - - -set MIXER fw_generic_wing.main.mix - -# Provide ESC a constant 1000 us pulse -set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha index d0cbc0368209..cc74225a1245 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha @@ -21,28 +21,21 @@ . ${R}etc/init.d/rc.fw_defaults - param set-default FW_AIRSPD_MAX 25 param set-default FW_AIRSPD_MIN 12.5 param set-default FW_AIRSPD_TRIM 16.5 -param set-default FW_L1_DAMPING 0.75 param set-default FW_L1_PERIOD 15 param set-default FW_LND_ANG 15 param set-default FW_LND_FLALT 8 -param set-default FW_LND_HHDIST 15 param set-default FW_LND_HVIRT 13 param set-default FW_LND_TLALT 10 param set-default FW_THR_LND_MAX 0 param set-default FW_P_LIM_MAX 20 param set-default FW_P_LIM_MIN -30 param set-default FW_R_LIM 45 -param set-default FW_R_TC 0.4 -param set-default FW_P_TC 0.4 param set-default FW_PR_FF 0.45 -param set-default FW_PR_IMAX 0.4 param set-default FW_PR_P 0.005 param set-default FW_RR_FF 0.45 -param set-default FW_RR_IMAX 0.2 param set-default FW_RR_P 0.013 param set-default FW_P_RMAX_NEG 70 param set-default FW_P_RMAX_POS 70 @@ -63,6 +56,6 @@ param set-default PWM_MAIN_REV2 1 param set-default PWM_MAIN_MIN 900 param set-default PWM_MAIN_MAX 2100 +set MIXER fw_generic_wing -set MIXER caipi set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x index cfa2ab089b26..fa2fd6d9e9e0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -22,6 +22,15 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.15 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.15 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.15 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.15 +param set-default CA_ROTOR3_KM -0.05 -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4003_qavr5 b/ROMFS/px4fmu_common/init.d/airframes/4003_qavr5 index ef87133ba296..f1a3a33d3ba9 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4003_qavr5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4003_qavr5 @@ -13,22 +13,12 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default MC_ROLL_P 8 param set-default MC_ROLLRATE_P 0.08 param set-default MC_ROLLRATE_I 0.16 -param set-default MC_ROLLRATE_D 0.003 param set-default MC_PITCH_P 8 param set-default MC_PITCHRATE_P 0.1 -param set-default MC_PITCHRATE_I 0.2 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_YAW_P 2.8 param set-default MC_YAWRATE_P 0.15 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 param set-default MPC_THR_MIN 0.06 param set-default MPC_MANTHR_MIN 0.06 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4009_qav250 b/ROMFS/px4fmu_common/init.d/airframes/4009_qav250 index 588d735c36c8..4efe6c7c5a8a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4009_qav250 @@ -13,10 +13,6 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default ATT_BIAS_MAX 0 param set-default CBRK_IO_SAFETY 22027 @@ -33,7 +29,6 @@ param set-default MC_PITCHRATE_D 0.004 param set-default MC_YAW_P 4 param set-default MC_YAWRATE_P 0.3 param set-default MC_YAWRATE_I 0.2 -param set-default MC_YAWRATE_D 0 param set-default MPC_THR_MIN 0.06 param set-default PWM_MAIN_MIN 1075 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/airframes/4010_dji_f330 deleted file mode 100644 index 2245ac8d19d9..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4010_dji_f330 +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/sh -# -# @name DJI F330 w/ DJI ESCs -# -# @type Quadrotor x -# @class Copter -# -# @maintainer Lorenz Meier -# - -. ${R}etc/init.d/rc.mc_defaults - -set MIXER quad_x -set PWM_OUT 1234 - -param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 -param set-default MC_ROLLRATE_I 0.05 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 -param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 - -# DJI ESCs do not support calibration and need a higher min -param set-default PWM_MAIN_MIN 1230 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/airframes/4011_dji_f450 deleted file mode 100644 index 9aa4c79eb2a4..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4011_dji_f450 +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/sh -# -# @name DJI F450 w/ DJI ESCs -# -# @type Quadrotor x -# @class Copter -# -# @maintainer Lorenz Meier -# - -. ${R}etc/init.d/rc.mc_defaults - -set MIXER quad_x -set PWM_OUT 1234 - -param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 -param set-default MC_ROLLRATE_I 0.05 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 -param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.3 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 - -# DJI ESCs do not support calibration and need a higher min -param set-default PWM_MAIN_MIN 1230 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 index 72629b75e62c..2c75a46c250f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4014_s500 +++ b/ROMFS/px4fmu_common/init.d/airframes/4014_s500 @@ -13,13 +13,7 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default MC_ROLLRATE_P 0.18 param set-default MC_PITCHRATE_P 0.18 param set-default MC_ROLLRATE_I 0.15 param set-default MC_PITCHRATE_I 0.15 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_PITCHRATE_D 0.003 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 index 62d2fe9c9863..d16ec71d3f41 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 +++ b/ROMFS/px4fmu_common/init.d/airframes/4015_holybro_s500 @@ -13,12 +13,7 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default IMU_GYRO_CUTOFF 30 -param set-default IMU_DGYRO_CUTOFF 30 param set-default MC_ROLLRATE_P 0.14 param set-default MC_PITCHRATE_P 0.14 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index da92a1104377..feb06fc88a62 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -16,10 +16,6 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - # System parameters # use FMU motor outputs for less delay in the rate control loop param set-default SYS_USE_IO 0 @@ -33,7 +29,6 @@ param set-default EKF2_AID_MASK 35 param set-default EKF2_IMU_POS_X 0.02 param set-default EKF2_GPS_POS_X 0.055 param set-default EKF2_GPS_POS_Z -0.15 -param set-default EKF2_HGT_MODE 0 param set-default EKF2_MIN_RNG 0.03 param set-default EKF2_OF_POS_X 0.055 param set-default EKF2_OF_POS_Y 0.02 @@ -41,7 +36,6 @@ param set-default EKF2_OF_POS_Z 0.065 param set-default EKF2_REQ_HDRIFT 0.3 param set-default EKF2_REQ_SACC 1 param set-default EKF2_REQ_VDRIFT 0.3 -param set-default EKF2_RNG_AID 1 param set-default EKF2_RNG_A_HMAX 8 param set-default EKF2_RNG_A_VMAX 2 param set-default EKF2_RNG_POS_X 0.055 @@ -66,15 +60,8 @@ param set-default MC_ACRO_R_MAX 200 param set-default MC_ACRO_SUPEXPO 0 param set-default MC_ACRO_SUPEXPOY 0 param set-default MC_ACRO_Y_MAX 150 -param set-default IMU_DGYRO_CUTOFF 30 param set-default MC_PITCHRATE_D 0.0015 -param set-default MC_PITCHRATE_I 0.2 -param set-default MC_PITCHRATE_K 1 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_ROLLRATE_D 0.0015 -param set-default MC_ROLLRATE_I 0.2 -param set-default MC_ROLLRATE_K 1 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_YAWRATE_P 0.3 # Position Control Tuning @@ -86,7 +73,6 @@ param set-default MPC_MANTHR_MIN 0 param set-default MPC_MAN_Y_MAX 120 param set-default MPC_TILTMAX_AIR 45 param set-default MPC_THR_HOVER 0.3 -param set-default MPC_THR_MIN 0.12 param set-default MPC_TKO_SPEED 1 param set-default MPC_VEL_MANUAL 5 param set-default MPC_XY_CRUISE 5 @@ -98,38 +84,33 @@ 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 param set-default NAV_ACC_RAD 2 -# PWM and RC Parameters -param set-default PWM_MAIN_MAX 1950 -param set-default PWM_MAIN_MIN 1075 - # use oneshot motor output protocol param set-default PWM_MAIN_RATE 0 # RTL Parameters param set-default RTL_DESCEND_ALT 5 param set-default RTL_RETURN_ALT 5 -param set-default RTL_CONE_ANG 45 # Logging Parameters param set-default SDLOG_PROFILE 131 # Sensors Parameters -param set-default SENS_BOARD_ROT 0 param set-default SENS_CM8JL65_CFG 104 param set-default SENS_FLOW_MAXHGT 25 param set-default SENS_FLOW_MINHGT 0.5 -param set-default SENS_FLOW_ROT 0 param set-default IMU_GYRO_CUTOFF 100 param set-default SENS_EN_PMW3901 1 # Power Parameters -param set-default BAT_N_CELLS 4 -param set-default BAT_A_PER_V 36.364 -param set-default BAT_V_DIV 18.182 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_A_PER_V 36.364 +param set-default BAT1_V_DIV 18.182 # Circuit breakers param set-default CBRK_IO_SAFETY 22027 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames index d28edc165ee4..dcfe0f943ba0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames +++ b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames @@ -18,16 +18,10 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - -param set-default IMU_GYRO_CUTOFF 40 param set-default IMU_DGYRO_CUTOFF 20 param set-default MC_ROLLRATE_P 0.18 param set-default MC_ROLLRATE_I 0.15 -param set-default MC_ROLLRATE_D 0.003 param set-default MC_PITCHRATE_P 0.18 param set-default MC_PITCHRATE_I 0.15 -param set-default MC_PITCHRATE_D 0.003 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc deleted file mode 100644 index 99dbec460975..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc +++ /dev/null @@ -1,56 +0,0 @@ -#!/bin/sh -# -# @name S500 with control allocation -# -# @type Quadrotor x -# @class Copter -# -# @maintainer Silvan Fuhrer -# -# @board px4_fmu-v2 exclude -# - -. ${R}etc/init.d/rc.mc_defaults -. ${R}etc/init.d/rc.ctrlalloc - - -param set-default MPC_USE_HTE 0 - -param set-default VM_MASS 1.5 -param set-default VM_INERTIA_XX 0.03 -param set-default VM_INERTIA_YY 0.03 -param set-default VM_INERTIA_ZZ 0.05 - -param set-default CA_AIRFRAME 0 -param set-default CA_METHOD 1 -param set-default CA_ACT0_MIN 0.0 -param set-default CA_ACT1_MIN 0.0 -param set-default CA_ACT2_MIN 0.0 -param set-default CA_ACT3_MIN 0.0 -param set-default CA_ACT0_MAX 1.0 -param set-default CA_ACT1_MAX 1.0 -param set-default CA_ACT2_MAX 1.0 -param set-default CA_ACT3_MAX 1.0 - -param set-default CA_MC_R0_PX 0.177 -param set-default CA_MC_R0_PY 0.177 -param set-default CA_MC_R0_CT 6.5 -param set-default CA_MC_R0_KM 0.05 -param set-default CA_MC_R1_PX -0.177 -param set-default CA_MC_R1_PY -0.177 -param set-default CA_MC_R1_CT 6.5 -param set-default CA_MC_R1_KM 0.05 -param set-default CA_MC_R2_PX 0.177 -param set-default CA_MC_R2_PY -0.177 -param set-default CA_MC_R2_CT 6.5 -param set-default CA_MC_R2_KM -0.05 -param set-default CA_MC_R3_PX -0.177 -param set-default CA_MC_R3_PY 0.177 -param set-default CA_MC_R3_CT 6.5 -param set-default CA_MC_R3_KM -0.05 - -set MIXER direct -set PWM_OUT 1234 - -set MIXER_AUX direct_aux -set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 new file mode 100644 index 000000000000..7cef516c4141 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name Holybro X500 V2 +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Farhang Naderi +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +param set-default IMU_GYRO_CUTOFF 30 + +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.3 +param set-default MC_PITCHRATE_I 0.3 +param set-default MC_ROLLRATE_D 0.004 +param set-default MC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb deleted file mode 100644 index a708e32d4d7b..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb +++ /dev/null @@ -1,38 +0,0 @@ -#!/bin/sh -# -# @name Hobbyking Micro PCB -# -# @type Quadrotor x -# @class Copter -# -# @maintainer Thomas Gubler -# -# @board px4_fmu-v2 exclude -# @board px4_fmu-v3 exclude -# @board px4_fmu-v4 exclude -# @board px4_fmu-v4pro exclude -# @board px4_fmu-v5 exclude -# @board px4_fmu-v5x exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.mc_defaults - -set MIXER quad_x -set PWM_OUT 1234 - - -param set-default MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.1 -param set-default MC_ROLLRATE_I 0.05 -param set-default MC_ROLLRATE_D 0.003 -param set-default MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.1 -param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.003 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 - -param set-default PWM_MAIN_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 96f78e8e1a84..f698daed991e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -18,7 +18,6 @@ . ${R}etc/init.d/rc.mc_defaults - # tuning param set-default MC_PITCHRATE_P 0.11 param set-default MC_ROLLRATE_P 0.11 @@ -48,7 +47,6 @@ param set-default CBRK_IO_SAFETY 22027 param set-default CBRK_SUPPLY_CHK 894281 # RC configuration -param set-default RC_MAP_MODE_SW 5 param set-default RC_MAP_PITCH 2 param set-default RC_MAP_ROLL 1 param set-default RC_MAP_THROTTLE 3 @@ -84,7 +82,5 @@ param set-default RC5_TRIM 1500 param set-default MAV_0_RATE 80000 param set-default MAV_0_MODE 2 param set-default SER_TEL1_BAUD 921600 -set MIXER quad_x -set PWM_OUT 1234 set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/init.d/airframes/4031_3dr_quad b/ROMFS/px4fmu_common/init.d/airframes/4031_3dr_quad index 405627f40ac6..e29fbf8dd663 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4031_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/airframes/4031_3dr_quad @@ -13,11 +13,6 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - -param set-default MC_ROLL_P 6.5 param set-default MC_ROLLRATE_P 0.14 param set-default MC_ROLLRATE_I 0.1 param set-default MC_ROLLRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4040_reaper b/ROMFS/px4fmu_common/init.d/airframes/4040_reaper index bf6bd46aa667..45ec3f081b44 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4040_reaper +++ b/ROMFS/px4fmu_common/init.d/airframes/4040_reaper @@ -20,8 +20,6 @@ . ${R}etc/init.d/rc.mc_defaults - -param set-default MC_ROLL_P 6.5 param set-default MC_ROLLRATE_P 0.14 param set-default MC_ROLLRATE_I 0.1 param set-default MC_ROLLRATE_D 0.004 @@ -33,7 +31,7 @@ param set-default MC_YAW_P 4 param set-default NAV_ACC_RAD 2 -param set-default PWM_AUX_DISARMED 950 +param set-default PWM_AUX_DISARM 950 param set-default PWM_AUX_RATE 50 param set-default PWM_MAIN_MIN 1100 param set-default PWM_MAIN_MAX 1900 @@ -44,8 +42,4 @@ param set-default RTL_DESCEND_ALT 10 set MIXER quad_h -set PWM_OUT 1234 - set MIXER_AUX pass - -set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index beb21c29379a..a04cc31f985b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -23,7 +23,6 @@ . ${R}etc/init.d/rc.mc_defaults - param set-default CBRK_SUPPLY_CHK 894281 param set-default CBRK_USB_CHK 197848 @@ -54,8 +53,6 @@ param set-default DSHOT_CONFIG 600 param set-default SYS_HAS_BARO 0 param set-default SYS_HAS_MAG 0 -param set-default BAT_N_CELLS 2 +param set-default BAT1_N_CELLS 2 # The Whoop uses reversed props set MIXER quad_h -set PWM_OUT 1234 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index 0837d6f7839b..dfd2bacd61ad 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -12,22 +12,15 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default MC_ROLL_P 8 param set-default MC_ROLLRATE_P 0.08 param set-default MC_ROLLRATE_I 0.25 param set-default MC_ROLLRATE_D 0.001 param set-default MC_PITCH_P 8 param set-default MC_PITCHRATE_P 0.08 -param set-default MC_PITCHRATE_I 0.25 param set-default MC_PITCHRATE_D 0.001 param set-default MC_YAW_P 4 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 + param set-default MC_ROLLRATE_MAX 1600 param set-default MC_PITCHRATE_MAX 1600 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq b/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq index 40230bb59bfb..74c99d69d358 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq +++ b/ROMFS/px4fmu_common/init.d/airframes/4051_s250aq @@ -23,10 +23,6 @@ . ${R}etc/init.d/rc.mc_defaults set MIXER quad_s250aq -set MAV_TYPE 2 - -set PWM_OUT 1234 - param set-default ATT_BIAS_MAX 0 @@ -41,9 +37,8 @@ param set-default MC_PITCHRATE_P 0.19 param set-default MC_PITCHRATE_I 0.1 param set-default MC_PITCHRATE_D 0.0055 param set-default MC_YAW_P 4 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 + + param set-default MC_ROLLRATE_MAX 720 param set-default MC_PITCHRATE_MAX 720 param set-default MC_YAWRATE_MAX 400 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 index df5fa21481df..aeb48c41d7db 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 @@ -15,12 +15,8 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - # The set does not include a battery, but most people will probably use 4S -param set-default BAT_N_CELLS 4 +param set-default BAT1_N_CELLS 4 param set-default IMU_GYRO_CUTOFF 120 param set-default IMU_DGYRO_CUTOFF 45 @@ -48,7 +44,6 @@ param set-default MPC_THR_HOVER 0.25 param set-default MPC_THR_MIN 0.05 param set-default MPC_Z_VEL_I_ACC 1.7 -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_MIN 1050 param set-default PWM_MAIN_RATE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 index 41a287c4c3ac..24a9fa9c46a7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 @@ -13,11 +13,7 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - -param set-default BAT_N_CELLS 4 +param set-default BAT1_N_CELLS 4 param set-default GPS_1_CONFIG 0 param set-default RC_PORT_CONFIG 201 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4060_dji_matrice_100 b/ROMFS/px4fmu_common/init.d/airframes/4060_dji_matrice_100 deleted file mode 100644 index 7ea158a44a01..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4060_dji_matrice_100 +++ /dev/null @@ -1,34 +0,0 @@ -#!/bin/sh -# -# @name DJI Matrice 100 -# -# @type Quadrotor x -# @class Copter -# -# @maintainer James Goppert -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.mc_defaults - -set MIXER quad_x -set PWM_OUT 1234 - - -param set-default BAT_N_CELLS 6 - -param set-default MC_ROLL_P 6.5 -param set-default MC_ROLLRATE_P 0.05 -param set-default MC_ROLLRATE_I 0.05 -param set-default MC_ROLLRATE_D 0.001 -param set-default MC_PITCH_P 6.5 -param set-default MC_PITCHRATE_P 0.05 -param set-default MC_PITCHRATE_I 0.05 -param set-default MC_PITCHRATE_D 0.001 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0 -param set-default MC_YAWRATE_D 0 - -param set-default PWM_MAIN_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu new file mode 100644 index 000000000000..2dacd54acbd2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -0,0 +1,180 @@ +#!/bin/sh +# +# @name Advanced Technology Labs (ATL) Mantis EDU +# +# @type Quadrotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# +# @maintainer +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +# We set the mixer and ESC manually. +set MIXER skip +set MIXER_AUX skip +set MIXER_FILE "" + +# Battery settings +param set-default BAT_CRIT_THR 0.20 +param set-default BAT_LOW_THR 0.25 + +param set-default BAT1_CAPACITY 2800.0 +param set-default BAT1_N_CELLS 3 +param set-default BAT1_R_INTERNAL 0.02 +param set-default BAT1_V_CHARGED 4.26 +param set-default BAT1_V_EMPTY 3.45 + +param set-default CBRK_SUPPLY_CHK 894281 + +param set-default COM_DISARM_LAND 0.1 +param set-default COM_DISARM_PRFLT 3 +param set-default COM_DL_LOSS_T 10 +param set-default COM_FLTMODE1 2 +param set-default COM_FLTMODE2 -1 +param set-default COM_FLTMODE3 -1 +param set-default COM_FLTMODE4 -1 +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_AID_MASK 33 +param set-default EKF2_TERR_MASK 1 +param set-default EKF2_BARO_DELAY 0 +param set-default EKF2_BARO_NOISE 2.0 + +param set-default EKF2_BCOEF_X 31.5 +param set-default EKF2_BCOEF_Y 25.5 + +param set-default EKF2_GPS_DELAY 100 +param set-default EKF2_GPS_POS_X 0.06 +param set-default EKF2_GPS_POS_Y 0.0 +param set-default EKF2_GPS_POS_Z 0.0 +param set-default EKF2_GPS_V_NOISE 0.5 + +param set-default EKF2_IMU_POS_X 0.06 + +param set-default EKF2_MAG_DELAY 0 +param set-default EKF2_MAG_NOISE 0.1 + +param set-default EKF2_MIN_RNG 0.15 + +param set-default EKF2_OF_DELAY 38 +param set-default EKF2_OF_GATE 2.0 +param set-default EKF2_OF_POS_X -0.035 +param set-default EKF2_OF_POS_Y 0.0 +param set-default EKF2_OF_POS_Z 0.033 +param set-default EKF2_OF_MIN_RNG 0.01 +param set-default EKF2_OF_A_HMAX 7.0 +param set-default EKF2_OF_QMIN 30 + +param set-default EKF2_PCOEF_XN -0.3 +param set-default EKF2_PCOEF_XP -0.4 +param set-default EKF2_PCOEF_YN -0.4 +param set-default EKF2_PCOEF_YP -0.4 + +param set-default EKF2_RNG_A_VMAX 1.0 +param set-default EKF2_RNG_AID 0 +param set-default EKF2_RNG_DELAY 55 +param set-default EKF2_RNG_POS_X -0.035 +param set-default EKF2_RNG_POS_Y 0.0 +param set-default EKF2_RNG_POS_Z 0.033 + +param set-default EKF2_TERR_NOISE 1.0 + + +# Maximum allowed angle velocity in the landed state +param set-default LNDMC_ROT_MAX 40.0 + +# 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 +param set-default MC_PITCHRATE_D 0.0005 +param set-default MC_PITCHRATE_MAX 360.0 +param set-default MC_PITCH_P 8.0 + +# Roll angle & rate setting +param set-default MC_ROLLRATE_P 0.055 +param set-default MC_ROLLRATE_I 0.1 +param set-default MC_ROLLRATE_D 0.0005 +param set-default MC_ROLLRATE_MAX 360.0 +param set-default MC_ROLL_P 8.0 + +# Yaw angle & rate setting +param set-default MC_YAWRATE_P 0.1 +param set-default MC_YAWRATE_MAX 120.0 +param set-default MC_YAW_P 2.5 + +param set-default MPC_ACC_DOWN_MAX 2.0 +param set-default MPC_ACC_HOR 3.0 +param set-default MPC_ACC_HOR_MAX 10.0 +param set-default MPC_ACC_UP_MAX 3.0 +param set-default MPC_ALT_MODE 0 +param set-default MPC_LAND_SPEED 0.5 +param set-default MPC_LAND_VEL_XY 10 +param set-default MPC_MAN_TILT_MAX 20 +param set-default MPC_YAWRAUTO_MAX 80.0 +param set-default MPC_POS_MODE 4 +param set-default MPC_THR_HOVER 0.54 +param set-default MPC_THR_MAX 0.9 +param set-default MPC_THR_MIN 0.06 +param set-default MPC_TILTMAX_AIR 30 +param set-default MPC_XY_P 1.0 +param set-default MPC_XY_VEL_D 0.005 +param set-default MPC_XY_VEL_I 0.02 +param set-default MPC_XY_VEL_P 0.15 +param set-default MPC_Z_P 2.0 +param set-default MPC_Z_VEL_D 0.0 +param set-default MPC_Z_VEL_I 0.02 +param set-default MPC_Z_VEL_MAX_DN 2.0 +param set-default MPC_Z_VEL_P 0.27 + + +# gimbal configuration +param set-default MNT_MODE_IN 0 +param set-default MNT_MODE_OUT 1 +param set-default MNT_MAN_PITCH 2 +param set-default MNT_RC_IN_MODE 1 +param set-default MNT_RATE_PITCH 30 + +# RC +param set-default RC_CHAN_CNT 12 + +param set-default RC_MAP_THROTTLE 1 +param set-default RC_MAP_YAW 2 +param set-default RC_MAP_PITCH 3 +param set-default RC_MAP_ROLL 4 +param set-default RC_MAP_AUX2 5 +param set-default RC_MAP_AUX3 10 +param set-default RC_MAP_AUX4 8 +param set-default RC_MAP_FLTMODE 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 +param set-default SENS_FLOW_MAXHGT 5.0 +param set-default SENS_FLOW_ROT 0 + +# ignore the SD card errors and use normal startup sound +set STARTUP_TUNE "1" diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index eb3e0a680f26..c2147e2d1f5a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -21,54 +21,25 @@ # @board cuav_x7pro exclude # -set VEHICLE_TYPE mc -set MIXER quad_x -set PWM_OUT 1234 - +. ${R}etc/init.d/rc.mc_defaults # Attitude & rate gains -#param set MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 -#param set MC_ROLLRATE_I 0.9 param set-default MC_ROLLRATE_D 0.0013 - -#param set MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 -#param set MC_PITCHRATE_I 1.1 param set-default MC_PITCHRATE_D 0.0016 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -#param set MC_YAWRATE_I 0.15 -param set-default MC_YAWRATE_D 0 param set-default MC_YAW_FF 0.5 -#param set MC_ROLL_TC 0.19 -#param set MC_PITCH_TC 0.16 - -# Manual mode settings: Unleash Draco R's power :) -#param set MPC_MAN_TILT_MAX 70 -#param set MC_PITCHRATE_MAX 1600 -#param set MC_ROLLRATE_MAX 1600 -#param set MC_YAWRATE_MAX 700 param set-default MPC_MANTHR_MAX 0.9 param set-default MPC_MANTHR_MIN 0.08 -#param set MPC_MAN_TILT_MAX 35 -#param set MPC_TILTMAX_AIR 20 # Filter settings param set-default IMU_DGYRO_CUTOFF 90 param set-default IMU_GYRO_CUTOFF 100 -# Thrust curve (avoids the need for TPA) -#param set THR_MDL_FAC 0.25 - # System -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_MIN 1100 param set-default PWM_MAIN_RATE 0 -#param set SYS_FMU_TASK 1 param set-default SENS_BOARD_ROT 10 # EKF2 @@ -92,11 +63,10 @@ param set-default MPC_TKO_RAMP_T 1 param set-default MPC_TKO_SPEED 1.1 param set-default MPC_VEL_MANUAL 3 -param set-default BAT_SOURCE 0 -param set-default BAT_N_CELLS 4 -param set-default BAT_V_DIV 10.14 -param set-default BAT_A_PER_V 18.18 -#param set CBRK_IO_SAFETY 22027 +param set-default BAT1_SOURCE 0 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_DIV 10.14 +param set-default BAT1_A_PER_V 18.18 param set-default COM_DISARM_LAND 2 # Filter settings @@ -106,9 +76,6 @@ param set-default IMU_DGYRO_CUTOFF 70 # Don't try to be intelligent on RC loss: just cut the motors param set-default NAV_RCL_ACT 6 -# enable to use high-rate logging for better rate tracking analysis -# param set SDLOG_PROFILE 19 - # TELEM1 ttyS1 - Wifi module param set-default MAV_0_CONFIG 101 param set-default MAV_0_RATE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco index 18afd3735fe9..4181f9f03ffd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4072_draco +++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco @@ -22,10 +22,6 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - # use the Q attitude estimator, it works w/o mag or GPS. param set-default SYS_MC_EST_GROUP 3 param set-default ATT_ACC_COMP 0 @@ -65,7 +61,6 @@ param set-default MPC_THR_HOVER 0.25 param set-default THR_MDL_FAC 0.3 # System -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_MIN 1180 # enable one-shot @@ -73,25 +68,12 @@ param set-default PWM_MAIN_RATE 0 param set-default SENS_BOARD_ROT 2 -param set-default BAT_SOURCE 0 +param set-default BAT1_SOURCE 0 param set-default CBRK_IO_SAFETY 22027 -#param set COM_DISARM_LAND 3 # Filter settings param set-default IMU_GYRO_CUTOFF 90 param set-default IMU_DGYRO_CUTOFF 100 -# Don't try to be intelligent on RC loss: just cut the motors -#param set NAV_RCL_ACT 6 - # enable to use high-rate logging for better rate tracking analysis param set-default SDLOG_PROFILE 27 - -# TELEM1 ttyS1 -#param set MAV_0_CONFIG 101 -#param set MAV_0_MODE 2 # onboard -#param set MAV_0_RATE 20000 -#param set SER_TEL1_BAUD 921600 - -# TELEM2 ttyS2 -#param set MAV_1_CONFIG 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index 3d9a7c34d432..bcafa1597285 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -21,50 +21,19 @@ # @maintainer Hyon Lim # -set VEHICLE_TYPE mc -set MIXER quad_x -set PWM_OUT 1234 - +. ${R}etc/init.d/rc.mc_defaults # Attitude & rate gains -#param set MC_ROLL_P 7 -param set-default MC_ROLLRATE_P 0.15 -#param set MC_ROLLRATE_I 0.9 param set-default MC_ROLLRATE_D 0.0013 -#param set MC_PITCH_P 7 -param set-default MC_PITCHRATE_P 0.15 -#param set MC_PITCHRATE_I 1.1 param set-default MC_PITCHRATE_D 0.0016 -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -#param set MC_YAWRATE_I 0.15 -param set-default MC_YAWRATE_D 0 - -#param set MC_ROLL_TC 0.19 -#param set MC_PITCH_TC 0.16 -# Manual mode settings: Unleash Draco R's power :) -#param set MPC_MAN_TILT_MAX 70 -#param set MC_PITCHRATE_MAX 1600 -#param set MC_ROLLRATE_MAX 1600 -#param set MC_YAWRATE_MAX 700 param set-default MPC_MANTHR_MIN 0.08 -#param set MPC_MAN_TILT_MAX 35 -#param set MPC_TILTMAX_AIR 20 # Filter settings param set-default IMU_GYRO_CUTOFF 100 -# Thrust curve (avoids the need for TPA) -#param set THR_MDL_FAC 0.25 - -# Obsolete -#param set PWM_MAIN_MAX 1950 -#param set PWM_MAIN_MIN 1100 -#param set PWM_MAIN_RATE 0 - # Sensors param set-default SENS_BOARD_ROT 10 # Yaw 180 @@ -80,7 +49,6 @@ param set-default EKF2_AID_MASK 3 param set-default EKF2_GND_EFF_DZ 6 param set-default EKF2_HGT_MODE 1 param set-default EKF2_MIN_RNG 0.3 -param set-default EKF2_RNG_AID 1 # Flow param set-default EKF2_OF_QMIN 70 @@ -102,11 +70,10 @@ param set-default MPC_TKO_RAMP_T 1 param set-default MPC_TKO_SPEED 1.1 param set-default MPC_VEL_MANUAL 3 -param set-default BAT_SOURCE 0 -param set-default BAT_N_CELLS 4 -param set-default BAT_V_DIV 10.14 -param set-default BAT_A_PER_V 18.18 -#param set CBRK_IO_SAFETY 22027 +param set-default BAT1_SOURCE 0 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_DIV 10.14 +param set-default BAT1_A_PER_V 18.18 param set-default COM_DISARM_LAND 2 # Filter settings @@ -115,9 +82,6 @@ param set-default IMU_GYRO_CUTOFF 90 # Don't try to be intelligent on RC loss: just cut the motors param set-default NAV_RCL_ACT 6 -# enable to use high-rate logging for better rate tracking analysis -# param set SDLOG_PROFILE 19 - # TELEM1 ttyS1 - Wifi module param set-default MAV_0_CONFIG 101 param set-default MAV_0_RATE 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 index 9576fd24be88..1a4ad19fbb7f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 @@ -21,25 +21,18 @@ param set-default CBRK_IO_SAFETY 22027 param set-default MC_ROLL_P 2.2 param set-default MC_ROLLRATE_P 0.06 -param set-default MC_ROLLRATE_I 0.2 param set-default MC_ROLLRATE_D 0.0017 param set-default MC_PITCH_P 2.2 param set-default MC_PITCHRATE_P 0.06 -param set-default MC_PITCHRATE_I 0.2 param set-default MC_PITCHRATE_D 0.0017 param set-default MC_YAW_P 1 param set-default MC_YAWRATE_P 0.15 param set-default MC_YAWRATE_I 0.2 -param set-default MC_YAWRATE_D 0 param set-default MC_ACRO_R_MAX 1000 param set-default MC_ACRO_P_MAX 1000 param set-default MC_ACRO_Y_MAX 1000 -# param set NAV_RCL_ACT 6 # Lockdown - -param set-default PWM_MAIN_MIN 1075 -param set-default PWM_MAIN_RATE 400 param set-default PWM_MAIN_DISARM 900 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind index a51335b8fa92..5215931a8400 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind +++ b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind @@ -17,11 +17,7 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - -param set-default BAT_N_CELLS 1 +param set-default BAT1_N_CELLS 1 param set-default MC_ROLL_P 8 param set-default MC_ROLLRATE_P 0.19 @@ -32,9 +28,6 @@ param set-default MC_PITCHRATE_P 0.19 param set-default MC_PITCHRATE_I 0.1 param set-default MC_PITCHRATE_D 0.0055 param set-default MC_YAW_P 4 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 param set-default PWM_MAIN_DISARM 0 param set-default PWM_MAIN_MIN 500 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4100_tiltquadrotor b/ROMFS/px4fmu_common/init.d/airframes/4100_tiltquadrotor index 3fd3f11d74f9..d9d8a8bb6303 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4100_tiltquadrotor +++ b/ROMFS/px4fmu_common/init.d/airframes/4100_tiltquadrotor @@ -24,15 +24,6 @@ . ${R}etc/init.d/rc.mc_defaults -#Parameters here: -param set LED_RGB_MAXBRT 8 - - -# Configure this as Quadrotor -# set MAV_TYPE 14 - # Set mixer set MIXER tilt_quad set MIXER_AUX tilt_quad - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal deleted file mode 100644 index 117d4037aa96..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ /dev/null @@ -1,198 +0,0 @@ -#!/bin/sh -# -# @name Teal One -# -# @type Quadrotor x -# @class Copter -# -# @output MAIN1 motor 1 -# @output MAIN2 motor 2 -# @output MAIN3 motor 3 -# @output MAIN4 motor 4 -# -# @maintainer Matt McFadden -# -# @board px4_fmu-v2 exclude -# @board px4_fmu-v3 exclude -# @board px4_fmu-v4pro exclude -# @board px4_fmu-v5 exclude -# @board px4_fmu-v5x exclude -# @board bitcraze_crazyflie exclude -# - -echo "Executing 4250_teal script." - -. ${R}etc/init.d/rc.mc_defaults - -set MIXER quad_x -set PWM_OUT 1234 - - -# First thing, reset all params to default... EXCEPT THIS LIST -param reset_all SYS_AUTOSTART SYS_AUTOCONFIG RC* COM_FLTMODE* LND_FLIGHT_T_* TC_* CAL_ACC* CAL_GYRO* CAL_MAG* SENS_BOARD* EKF2_MAGBIAS* - -# battery -param set-default BAT_CAPACITY 2750 -param set-default BAT_CRIT_THR 0.15 -param set-default BAT_EMERGEN_THR 0.075 -param set-default BAT_LOW_THR 0.20 -param set-default BAT_N_CELLS 4 -param set-default BAT_R_INTERNAL 0.06 -param set-default BAT_SOURCE 1 -param set-default BAT_V_CHARGED 4.15 -param set-default BAT_V_DIV 11.1625 -param set-default BAT_V_EMPTY 3.65 -param set-default BAT_V_OFFS_CURR -0.0045 - -# sensor calibration -param set-default CAL_MAG_SIDES 63 -param set-default SENS_BOARD_ROT 0 - -# circuit breakers -param set-default CBRK_IO_SAFETY 22027 -param set-default CBRK_USB_CHK 197848 - -# commander -param set-default COM_DISARM_LAND 0.1 -# Return mode at critically low level, Land mode at current position if reaching dangerously low levels. -param set-default COM_LOW_BAT_ACT 3 - -# ekf2 -param set-default EKF2_AID_MASK 1 -param set-default EKF2_MAG_TYPE 1 -param set-default EKF2_GPS_CHECK 511 -param set-default EKF2_GPS_POS_X -0.04 -param set-default EKF2_IMU_POS_X -0.06 -param set-default EKF2_PCOEF_XN 0.1 -param set-default EKF2_PCOEF_XP -0.5 -param set-default EKF2_RNG_AID 1 -param set-default EKF2_RNG_A_VMAX 20 -param set-default EKF2_RNG_NOISE 0.2 -param set-default EKF2_MIN_RNG 0.07 - -# gps -param set-default GPS_UBX_DYNMODEL 7 - -# geofence -# Geofence violation action -- Warning. -param set-default GF_ACTION 1 - -# land detector -param set-default LNDMC_XY_VEL_MAX 1 -# This is set high because we have lots of vibrations while in contact with ground. -param set-default LNDMC_ROT_MAX 50 - -# serial comms -param set-default SER_TEL1_BAUD 921600 -param set-default SER_TEL2_BAUD 921600 - -# mavlink stream configuration -# TEL1 ttyS1 -- disabled. TX1 FTDI UART has issues. -param set-default MAV_0_CONFIG 0 -param set-default MAV_0_RATE 800000 - -# TEL2 ttyS2 -- Primary MAVLINK stream to companion computer. -param set-default MAV_1_CONFIG 102 -param set-default MAV_1_RATE 800000 - -# mc_att_control -param set-default MC_ACRO_P_MAX 360 -param set-default MC_ACRO_R_MAX 360 -param set-default MC_ACRO_Y_MAX 360 - -param set-default MC_ROLL_P 6 -param set-default MC_ROLLRATE_P 0.055 -param set-default MC_ROLLRATE_I 0.2 -param set-default MC_ROLLRATE_D 0.0012 -param set-default MC_ROLLRATE_MAX 180 - -param set-default MC_PITCH_P 6.5 -param set-default MC_PITCHRATE_P 0.06 -param set-default MC_PITCHRATE_I 0.2 -param set-default MC_PITCHRATE_D 0.0012 -param set-default MC_PITCHRATE_MAX 180 - -param set-default MC_YAW_P 1 -param set-default MC_YAWRATE_P 0.08 -param set-default MC_YAWRATE_I 0.08 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAWRATE_MAX 180 - -# Set to reduce voltage transients as seen by battery management system. -param set-default MOT_SLEW_MAX 0.15 - -#### CONTROLLER ### -param set-default MPC_LAND_ALT1 8 -param set-default MPC_LAND_ALT2 5 -param set-default MPC_TKO_RAMP_T 0.75 -param set-default MPC_TKO_SPEED 0.75 -param set-default MPC_TILTMAX_LND 18 -param set-default MPC_TILTMAX_AIR 45 -param set-default MPC_MAN_TILT_MAX 45 - -param set-default MPC_MANTHR_MAX 0.85 -param set-default MPC_MANTHR_MIN 0.15 -# Full throttle can trip over current protection on BMS. -param set-default MPC_THR_MAX 0.85 -param set-default MPC_THR_MIN 0.15 - -param set-default MPC_VEL_MANUAL 26.5 -# RTL speed, it was too fast and scaring people. -param set-default MPC_XY_CRUISE 15 - -param set-default MPC_MAN_Y_MAX 200 - -param set-default MPC_JERK_MAX 5 -param set-default MPC_ACC_UP_MAX 10 -param set-default MPC_ACC_DOWN_MAX 10 -param set-default MPC_ACC_HOR 10 -param set-default MPC_ACC_HOR_MAX 15 - -param set-default MPC_XY_P 1.15 -param set-default MPC_XY_VEL_P_ACC 2.8 -param set-default MPC_XY_VEL_I_ACC 0.28 -param set-default MPC_XY_VEL_D_ACC 0.28 -param set-default MPC_XY_VEL_MAX 26.5 - -param set-default MPC_Z_P 0.85 -param set-default MPC_Z_VEL_P_ACC 5 -param set-default MPC_Z_VEL_I_ACC 1.7 -param set-default MPC_Z_VEL_D_ACC 0.4 -# Documentation says limit is 8.0, but does not seem to be enforced in code. -param set-default MPC_Z_VEL_MAX_UP 20 -param set-default MPC_Z_VEL_MAX_DN 2.5 -#### CONTROLLER ### - -# navigator -param set-default NAV_ACC_RAD 2.5 -# RC loss failsafe behavior -- hold mode. -param set-default NAV_RCL_ACT 1 - -# pwm control -param set-default PWM_MAIN_DISARM 900 -param set-default PWM_MAIN_MAX 1850 -param set-default PWM_MAIN_MIN 1075 -# Oneshot125 -param set-default PWM_MAIN_RATE 0 - -# rtl -param set-default RTL_DESCEND_ALT 5 -param set-default RTL_LAND_DELAY 5 -param set-default RTL_MIN_DIST 7.5 -param set-default RTL_RETURN_ALT 25 - -# calibration -param set-default CAL_ACC0_PRIO 255 -param set-default CAL_ACC1_PRIO 0 - -param set-default CAL_GYRO0_PRIO 255 -param set-default CAL_GYRO1_PRIO 0 -# Logger mode. Default(1) + estimator replay(2) + thermal calibration(4) -param set SDLOG_PROFILE 6 - -# Do not start frsky_telemetry on port ttyS6 by default, PGA460 lives there. 500 is in arbitrary unused number. -param set TEL_FRSKY_CONFIG 500 -# We want to make sure these always start -param set SENS_EN_PGA460 1 -param set SENS_EN_THERMAL 1 -param set SENS_EN_BATT 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 index 91a5ef23f0a1..d8116882fe6c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 +++ b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 @@ -13,10 +13,6 @@ . ${R}etc/init.d/rc.mc_defaults -set MIXER quad_x -set PWM_OUT 1234 - - param set-default MC_PITCHRATE_P 0.087 param set-default MC_PITCHRATE_I 0.037 param set-default MC_PITCHRATE_D 0.0044 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index da3c4bfac0db..cd7a74134b59 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -17,11 +17,10 @@ . ${R}etc/init.d/rc.mc_defaults set MIXER quad_x_cw -set PWM_OUT 1234 -param set-default BAT_N_CELLS 1 -param set-default BAT_CAPACITY 240 -param set-default BAT_SOURCE 1 +param set-default BAT1_N_CELLS 1 +param set-default BAT1_CAPACITY 240 +param set-default BAT1_SOURCE 1 param set-default CBRK_SUPPLY_CHK 894281 param set-default CBRK_USB_CHK 197848 @@ -39,13 +38,9 @@ param set-default IMU_ACCEL_CUTOFF 30 param set-default MC_AIRMODE 1 param set-default IMU_DGYRO_CUTOFF 70 param set-default MC_PITCHRATE_D 0.002 -param set-default MC_PITCHRATE_I 0.2 param set-default MC_PITCHRATE_P 0.07 -param set-default MC_PITCH_P 6.5 param set-default MC_ROLLRATE_D 0.002 -param set-default MC_ROLLRATE_I 0.2 param set-default MC_ROLLRATE_P 0.07 -param set-default MC_ROLL_P 6.5 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 @@ -65,8 +60,6 @@ param set-default PWM_MAIN_MAX 255 # Run the motors at 328.125 kHz (recommended) param set-default PWM_MAIN_RATE 3921 -param set-default SDLOG_PROFILE 1 - param set-default SENS_FLOW_MINRNG 0.05 set PWM_MAIN_DISARM none @@ -75,4 +68,3 @@ set PWM_MAIN_MIN none syslink start mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 67fb4406cef5..878a281a679e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -17,14 +17,12 @@ . ${R}etc/init.d/rc.mc_defaults set MIXER quad_x_cw -set PWM_OUT 1234 param set-default SYS_MC_EST_GROUP 2 param set-default SYS_HAS_MAG 0 param set-default EKF2_AID_MASK 2 param set-default EKF2_MAG_TYPE 5 -param set-default BAT_N_CELLS 1 param set-default BAT1_N_CELLS 1 param set-default BAT1_SOURCE 1 @@ -38,13 +36,9 @@ param set-default IMU_ACCEL_CUTOFF 30 param set-default MC_AIRMODE 1 param set-default IMU_DGYRO_CUTOFF 70 param set-default MC_PITCHRATE_D 0.002 -param set-default MC_PITCHRATE_I 0.2 param set-default MC_PITCHRATE_P 0.07 -param set-default MC_PITCH_P 6.5 param set-default MC_ROLLRATE_D 0.002 -param set-default MC_ROLLRATE_I 0.2 param set-default MC_ROLLRATE_P 0.07 -param set-default MC_ROLL_P 6.5 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 @@ -54,8 +48,6 @@ param set-default MPC_Z_VEL_P_ACC 8 param set-default MPC_Z_VEL_I_ACC 6 param set-default MPC_HOLD_MAX_XY 0.1 param set-default MPC_MAX_FLOW_HGT 3 -#param set IMU_GYRO_NF_FREQ 10 -#param set IMU_GYRO_NF_BW 20 param set-default NAV_RCL_ACT 3 @@ -66,11 +58,8 @@ param set-default PWM_MAIN_MAX 255 # Run the motors at 328.125 kHz (recommended) param set-default PWM_MAIN_RATE 3921 -param set-default SDLOG_PROFILE 1 - param set-default SENS_FLOW_MINRNG 0.05 - set PWM_MAIN_DISARM none set PWM_MAIN_MAX none set PWM_MAIN_MIN none diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle index 41242a98c5cf..2c79163b3486 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -16,7 +16,7 @@ . ${R}etc/init.d/rc.rover_defaults -param set-default BAT_N_CELLS 2 +param set-default BAT1_N_CELLS 2 param set-default EKF2_ANGERR_INIT 0.01 param set-default EKF2_GBIAS_INIT 0.01 @@ -48,8 +48,6 @@ param set-default NAV_ACC_RAD 0.5 param set-default PWM_MAIN_DISARM 1500 param set-default PWM_MAIN_MAX 2000 param set-default PWM_MAIN_MIN 1000 -# Configure this as rover -set MAV_TYPE 10 # Set mixer set MIXER rover_generic diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index f4999452c357..118e6915c9dd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -2,7 +2,7 @@ # # @name Aion Robotics R1 UGV # -# @url http://docs.aionrobotics.com/en/latest/r1-ugv.html +# @url https://www.aionrobotics.com/r1 # # @type Rover # @class Rover @@ -18,8 +18,7 @@ . ${R}etc/init.d/rc.rover_defaults - -param set-default BAT_N_CELLS 4 +param set-default BAT1_N_CELLS 4 param set-default EKF2_GBIAS_INIT 0.01 param set-default EKF2_ANGERR_INIT 0.01 @@ -72,15 +71,11 @@ param set-default GND_MAX_ANG 3.1415 param set-default RBCLW_BAUD 8 param set-default RBCLW_COUNTS_REV 1200 param set-default RBCLW_ADDRESS 128 -# param set SER_TEL4_BAUD 115200 # 104 corresponds to Telem 4 param set-default RBCLW_SER_CFG 104 # Start this driver after setting parameters, because the driver uses some of those parameters. # roboclaw start /dev/ttyS3 -# Configure this as rover -set MAV_TYPE 10 - # Set mixer set MIXER generic_diff_rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx index fd6f19560570..1c1d55b3928a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx @@ -20,8 +20,7 @@ . ${R}etc/init.d/rc.rover_defaults - -param set-default BAT_N_CELLS 2 +param set-default BAT1_N_CELLS 2 param set-default EKF2_GBIAS_INIT 0.01 param set-default EKF2_ANGERR_INIT 0.01 @@ -66,9 +65,6 @@ param set-default PWM_MAIN_MIN4 970 # Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor param set-default CBRK_AIRSPD_CHK 162128 -# Configure this as rover -set MAV_TYPE 10 - # Set mixer set MIXER rover_diff_and_servo diff --git a/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ b/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ index 5e5df33a4c64..3d172387c849 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/5001_quad_+ @@ -20,10 +20,9 @@ # @maintainer Lorenz Meier # # @board bitcraze_crazyflie exclude +# @board px4_fmu-v2 exclude # . ${R}etc/init.d/rc.mc_defaults set MIXER quad_+ - -set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic index 705d185a6a4f..e0b33a953492 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic +++ b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic @@ -13,15 +13,7 @@ . ${R}etc/init.d/rc.uuv_defaults - -#Set data link loss failsafe mode (0: disabled) -param set-default NAV_DLL_ACT 0 - # disable circuit breaker for airspeed sensor param set-default CBRK_AIRSPD_CHK 162128 -#param set CBRK_GPSFAIL 240024 -set MAV_TYPE 12 -param set MAV_TYPE ${MAV_TYPE} - set MIXER uuv_x diff --git a/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus index 360d700fd605..8ea864695624 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus +++ b/ROMFS/px4fmu_common/init.d/airframes/60001_uuv_hippocampus @@ -13,15 +13,7 @@ . ${R}etc/init.d/rc.uuv_defaults - -#Set data link loss failsafe mode (0: disabled) -param set-default NAV_DLL_ACT 0 - # disable circuit breaker for airspeed sensor param set-default CBRK_AIRSPD_CHK 162128 -#param set CBRK_GPSFAIL 240024 -set MAV_TYPE 12 -param set MAV_TYPE ${MAV_TYPE} - set MIXER uuv_x 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 69384284be64..44b876564d6f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d/airframes/60002_uuv_bluerov2_heavy @@ -23,9 +23,6 @@ . ${R}etc/init.d/rc.uuv_defaults -#Set data link loss failsafe mode (0: disabled) -param set-default NAV_DLL_ACT 0 - # disable circuit breaker for airspeed sensor param set-default CBRK_AIRSPD_CHK 162128 # companion computer is connected via USB permanently @@ -44,4 +41,3 @@ param set-default BAT_V_OFFS_CURR 0.33 set PWM_OUT 12345678 # set MIXER IO_pass set MIXER vectored6dof - diff --git a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x index 893bb991c9f6..c2bbb26cdbca 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x @@ -23,6 +23,26 @@ . ${R}etc/init.d/rc.mc_defaults +# MAV_TYPE_HEXAROTOR 13 +param set-default MAV_TYPE 13 + +param set-default CA_ROTOR_COUNT 6 +param set-default CA_ROTOR0_PX 0.0 +param set-default CA_ROTOR0_PY 0.5 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR1_PX 0.0 +param set-default CA_ROTOR1_PY -0.5 +param set-default CA_ROTOR2_PX 0.43 +param set-default CA_ROTOR2_PY -0.25 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.43 +param set-default CA_ROTOR3_PY 0.25 +param set-default CA_ROTOR4_PX 0.43 +param set-default CA_ROTOR4_PY 0.25 +param set-default CA_ROTOR5_PX -0.43 +param set-default CA_ROTOR5_PY -0.25 +param set-default CA_ROTOR5_KM -0.05 + set MIXER hexa_x # Need to set all 8 channels diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r index 1eefccbfb1da..70a88e3fdf58 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -26,31 +26,22 @@ # . ${R}etc/init.d/rc.mc_defaults -set MIXER hexa_x -set PWM_OUT 12345678 +# MAV_TYPE_HEXAROTOR 13 +param set-default MAV_TYPE 13 ############################################### # Attitude & rate gains ############################################### # Roll -param set-default MC_ROLL_P 6.5 -param set-default MC_ROLLRATE_P 0.15 param set-default MC_ROLLRATE_I 0.05 param set-default MC_ROLLRATE_D 0.0013 param set-default MC_ROLLRATE_MAX 800 # Pitch -param set-default MC_PITCH_P 6.5 -param set-default MC_PITCHRATE_P 0.15 param set-default MC_PITCHRATE_I 0.05 param set-default MC_PITCHRATE_D 0.0016 param set-default MC_PITCHRATE_MAX 800 # Yaw -param set-default MC_YAW_P 2.8 -param set-default MC_YAWRATE_P 0.2 -param set-default MC_YAWRATE_I 0.1 -param set-default MC_YAWRATE_D 0 -param set-default MC_YAW_FF 0 param set-default MC_YAWRATE_MAX 700 ############################################### @@ -70,7 +61,6 @@ param set-default MPC_XY_VEL_I_ACC 0.4 param set-default MPC_XY_VEL_D_ACC 0.2 # etc gains param set-default MPC_TKO_RAMP_T 0.4 -param set-default MPC_TKO_SPEED 1.5 param set-default MPC_VEL_MANUAL 5 # Filter settings @@ -81,7 +71,6 @@ param set-default IMU_GYRO_CUTOFF 100 param set-default THR_MDL_FAC 0.25 # System -param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_MIN 1100 param set-default PWM_MAIN_DIS5 980 param set-default PWM_MAIN_DIS6 980 @@ -97,8 +86,6 @@ param set-default MIS_TAKEOFF_ALT 1.1 # Height mode as GPS param set-default EKF2_HGT_MODE 1 # Enable optical flow and GPS -param set-default EKF2_AID_MASK 1 -param set-default EKF2_RNG_AID 1 param set-default EKF2_MAG_TYPE 1 param set-default EKF2_OF_QMIN 80 @@ -116,9 +103,9 @@ param set-default COM_DISARM_LAND 3 param set-default PWM_MAIN_RATE 0 # Battery -param set-default BAT_SOURCE 0 -param set-default BAT_N_CELLS 4 -param set-default BAT_V_DIV 10.133 +param set-default BAT1_SOURCE 0 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_DIV 10.133 # TELEM1 ttyS1 param set-default MAV_0_CONFIG 101 @@ -132,3 +119,7 @@ param set-default MAV_1_MODE 2 param set-default MAV_1_FORWARD 1 param set-default MAV_1_RATE 800000 param set-default SER_TEL2_BAUD 921600 + + +set MIXER hexa_x +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc b/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc deleted file mode 100644 index 00c31ab2ecf6..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc +++ /dev/null @@ -1,74 +0,0 @@ -#!/bin/sh -# -# @name Hex X with control allocation -# -# @type Hexarotor x -# @class Copter -# -# @maintainer Silvan Fuhrer -# -# @board px4_fmu-v2 exclude -# - -. ${R}etc/init.d/rc.mc_defaults -. ${R}etc/init.d/rc.ctrlalloc - - -param set-default MPC_USE_HTE 0 - -param set-default VM_MASS 1.5 -param set-default VM_INERTIA_XX 0.03 -param set-default VM_INERTIA_YY 0.03 -param set-default VM_INERTIA_ZZ 0.05 - -param set-default CA_AIRFRAME 0 -param set-default CA_METHOD 1 -param set-default CA_ACT0_MIN 0.0 -param set-default CA_ACT1_MIN 0.0 -param set-default CA_ACT2_MIN 0.0 -param set-default CA_ACT3_MIN 0.0 -param set-default CA_ACT4_MIN 0.0 -param set-default CA_ACT5_MIN 0.0 - -param set-default CA_ACT0_MAX 1.0 -param set-default CA_ACT1_MAX 1.0 -param set-default CA_ACT2_MAX 1.0 -param set-default CA_ACT3_MAX 1.0 -param set-default CA_ACT4_MAX 1.0 -param set-default CA_ACT5_MAX 1.0 - -param set-default CA_MC_R0_PX 0.0 -param set-default CA_MC_R0_PY 0.275 -param set-default CA_MC_R0_CT 6.5 -param set-default CA_MC_R0_KM -0.05 - -param set-default CA_MC_R1_PX 0.0 -param set-default CA_MC_R1_PY -0.275 -param set-default CA_MC_R1_CT 6.5 -param set-default CA_MC_R1_KM 0.05 - -param set-default CA_MC_R2_PX 0.238 -param set-default CA_MC_R2_PY -0.1375 -param set-default CA_MC_R2_CT 6.5 -param set-default CA_MC_R2_KM -0.05 - -param set-default CA_MC_R3_PX -0.238 -param set-default CA_MC_R3_PY 0.1375 -param set-default CA_MC_R3_CT 6.5 -param set-default CA_MC_R3_KM 0.05 - -param set-default CA_MC_R4_PX 0.238 -param set-default CA_MC_R4_PY 0.1375 -param set-default CA_MC_R4_CT 6.5 -param set-default CA_MC_R4_KM 0.05 - -param set-default CA_MC_R5_PX -0.238 -param set-default CA_MC_R5_PY -0.1375 -param set-default CA_MC_R5_CT 6.5 -param set-default CA_MC_R5_KM -0.05 - -set MIXER direct -set PWM_OUT 123456 - -set MIXER_AUX direct_aux -set PWM_AUX_OUT 123456 diff --git a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ index 8f9e6f7cf983..e1db21eca3f2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ @@ -23,6 +23,26 @@ . ${R}etc/init.d/rc.mc_defaults +# MAV_TYPE_HEXAROTOR 13 +param set-default MAV_TYPE 13 + +param set-default CA_ROTOR_COUNT 6 +param set-default CA_ROTOR0_PX 0.5 +param set-default CA_ROTOR0_PY 0.0 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR1_PX -0.5 +param set-default CA_ROTOR1_PY 0.0 +param set-default CA_ROTOR2_PX -0.25 +param set-default CA_ROTOR2_PY -0.43 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX 0.25 +param set-default CA_ROTOR3_PY 0.43 +param set-default CA_ROTOR4_PX 0.25 +param set-default CA_ROTOR4_PY -0.43 +param set-default CA_ROTOR5_PX -0.25 +param set-default CA_ROTOR5_PY 0.43 +param set-default CA_ROTOR5_KM -0.05 + set MIXER hexa_+ # Need to set all 8 channels diff --git a/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x b/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x index e69776f31497..cabae6997520 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x @@ -25,6 +25,31 @@ . ${R}etc/init.d/rc.mc_defaults +# MAV_TYPE_OCTOROTOR 14 +param set-default MAV_TYPE 14 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_ROTOR0_KM -0.05 +param set-default CA_ROTOR0_PX 0.46 +param set-default CA_ROTOR0_PY 0.19 +param set-default CA_ROTOR1_KM -0.05 +param set-default CA_ROTOR1_PX -0.46 +param set-default CA_ROTOR1_PY -0.19 +param set-default CA_ROTOR2_PX 0.19 +param set-default CA_ROTOR2_PY 0.46 +param set-default CA_ROTOR3_PX -0.46 +param set-default CA_ROTOR3_PY 0.19 +param set-default CA_ROTOR4_PX 0.46 +param set-default CA_ROTOR4_PY -0.19 +param set-default CA_ROTOR5_PX -0.19 +param set-default CA_ROTOR5_PY -0.46 +param set-default CA_ROTOR6_KM -0.05 +param set-default CA_ROTOR6_PX 0.19 +param set-default CA_ROTOR6_PY -0.46 +param set-default CA_ROTOR7_KM -0.05 +param set-default CA_ROTOR7_PX -0.19 +param set-default CA_ROTOR7_PY 0.46 + set MIXER octo_x set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ b/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ index b61a1f95b03a..95c24795602f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ @@ -25,6 +25,9 @@ . ${R}etc/init.d/rc.mc_defaults +# MAV_TYPE_OCTOROTOR 14 +param set-default MAV_TYPE 14 + set MIXER octo_+ set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f1ce40b6aa15..d99b2d8d2528 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -39,6 +39,8 @@ px4_add_romfs_files( 1001_rc_quad_x.hil 1002_standard_vtol.hil 1100_rc_quad_x_sih.hil + 1101_rc_plane_sih.hil + 1102_tailsitter_duo_sih.hil # [2000, 2999] Standard planes" 2100_standard_plane @@ -50,28 +52,21 @@ px4_add_romfs_files( # [3000, 3999] Flying wing" 3000_generic_wing - 3030_io_camflyer 3031_phantom 3032_skywalker_x5 3033_wingwing - 3034_fx79 - 3035_viper 3036_pigeon - 3037_parrot_disco_mod 3100_tbs_caipirinha # [4000, 4999] Quadrotor x" 4001_quad_x 4003_qavr5 4009_qav250 - 4010_dji_f330 - 4011_dji_f450 4014_s500 4015_holybro_s500 4016_holybro_px4vision 4017_nxp_hovergames - 4018_s500_ctrlalloc - 4020_hk_micro_pcb + 4019_x500_v2 4030_3dr_solo 4031_3dr_quad 4040_reaper @@ -80,14 +75,13 @@ px4_add_romfs_files( 4051_s250aq 4052_holybro_qav250 4053_holybro_kopis2 - 4060_dji_matrice_100 + 4061_atl_mantis_edu 4071_ifo 4072_draco 4073_ifo-s 4080_zmr250 4090_nanomind 4100_tiltquadrotor - 4250_teal 4500_clover4 4900_crazyflie 4901_crazyflie21 @@ -98,7 +92,6 @@ px4_add_romfs_files( # [6000, 6999] Hexarotor x" 6001_hexa_x 6002_draco_r - 6003_hexa_x_ctrlalloc # [7000, 7999] Hexarotor +" 7001_hexa_+ @@ -112,7 +105,6 @@ px4_add_romfs_files( # [10000, 10999] Quadrotor Wide arm / H frame" 10015_tbs_discovery 10016_3dr_iris - 10017_steadidrone_qu4d 10018_tbs_endurance # [11000, 11999] Hexa Cox @@ -120,7 +112,6 @@ px4_add_romfs_files( # [12000, 12999] Octo Cox 12001_octo_cox - 12002_steadidrone_mavrik # [13000, 13999] VTOL 13000_generic_vtol_standard @@ -133,7 +124,6 @@ px4_add_romfs_files( 13007_vtol_AAVVT_quad 13008_QuadRanger 13009_vtol_spt_ranger - 13010_claire 13012_convergence 13013_deltaquad 13014_vtol_babyshark diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index 766a17347e58..4031bd4f7142 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -48,6 +48,14 @@ fi # End Estimator Group Selection # ############################################################################### +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi + # # Start Airship Attitude Controller. # diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_defaults b/ROMFS/px4fmu_common/init.d/rc.airship_defaults index 25ac61e21213..8c51643915fc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.airship_defaults @@ -7,6 +7,9 @@ set VEHICLE_TYPE airship +# MAV_TYPE_AIRSHIP 7 +param set-default MAV_TYPE 7 + # # This is the gimbal pass mixer. # diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart_ext b/ROMFS/px4fmu_common/init.d/rc.autostart_ext new file mode 100644 index 000000000000..7f7916c7f90f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart_ext @@ -0,0 +1,12 @@ +#!/bin/sh +# +# External airframe startup script (on SD card) +# +set SDCARD_MIXERS_PATH ${SDCARD_EXT_PATH}/mixers + +if [ -e ${SDCARD_EXT_PATH}/rc.autostart ] +then + . ${SDCARD_EXT_PATH}/rc.autostart +else + echo "Error: ${SDCARD_EXT_PATH}/rc.autostart does not exist" +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults index 302d81ae9066..6f7d925e0c42 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults @@ -7,6 +7,9 @@ set VEHICLE_TYPE fw +# MAV_TYPE_FREE_BALLOON 8 +param set-default MAV_TYPE 8 + # # Default parameters for balloon UAVs. # diff --git a/ROMFS/px4fmu_common/init.d/rc.boat_defaults b/ROMFS/px4fmu_common/init.d/rc.boat_defaults index 54bb4d0ccdb7..a281b8395ed8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.boat_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.boat_defaults @@ -7,13 +7,15 @@ set VEHICLE_TYPE rover +# MAV_TYPE_SURFACE_BOAT 11 +param set-default MAV_TYPE 11 + # # Default parameters for UGVs. # param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 -param set-default NAV_DLL_ACT 0 param set-default NAV_ACC_RAD 2 # Temporary. diff --git a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc b/ROMFS/px4fmu_common/init.d/rc.ctrlalloc deleted file mode 100644 index e94b5fb16b59..000000000000 --- a/ROMFS/px4fmu_common/init.d/rc.ctrlalloc +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/sh -# -# Standard apps for new control allocation and controllers -# -# NOTE: Script variables are declared/initialized/unset in the rcS script. -# - -# -# Start angular velocity controller -# -angular_velocity_controller start -mc_rate_control stop - -# -# Start Control Allocator -# -control_allocator start - -# -# Disable hover thrust estimator and prearming -# These features are currently incompatible with control allocation -# -# TODO: fix -# -param set MPC_USE_HTE 0 -param set COM_PREARM_MODE 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 95c5a365dbeb..acc0fb27c5c4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -10,12 +10,26 @@ # ekf2 start & +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi + # # Start attitude controller. # fw_att_control start fw_pos_control_l1 start airspeed_selector start + +# +# Start attitude control auto-tuner +# +fw_autotune_attitude_control start + # # Start Land Detector. # diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index ca4f0f790500..6cae25306ead 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -7,15 +7,18 @@ set VEHICLE_TYPE fw +# MAV_TYPE_FIXED_WING 1 +param set-default MAV_TYPE 1 + # # Default parameters for fixed wing UAVs. # param set-default COM_POS_FS_DELAY 5 param set-default COM_POS_FS_EPH 15 param set-default COM_POS_FS_EPV 30 -param set-default COM_POS_FS_GAIN 0 -param set-default COM_POS_FS_PROB 1 param set-default COM_VEL_FS_EVH 5 +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 3bbcda974603..fee6ad252cb8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -17,6 +17,19 @@ set OUTPUT_DEV none set OUTPUT_AUX_DEV /dev/pwm_output1 set OUTPUT_EXTRA_DEV /dev/pwm_output0 +# set these before starting the modules +if [ $PWM_AUX_OUT != none ] +then + + param set PWM_AUX_OUT ${PWM_AUX_OUT} +fi + + +if [ $PWM_OUT != none ] +then + param set PWM_MAIN_OUT ${PWM_OUT} +fi + # # If mount (gimbal) control is enabled and output mode is AUX, set the aux # mixer to mount (override the airframe-specific MIXER_AUX setting). @@ -29,16 +42,10 @@ then fi fi -# USE_IO is set to 'no' for all boards w/o px4io driver or SYS_USE_IO disabled -if [ $USE_IO = no -a $AUX_BANK2 = none ] -then - set AUX_MODE none -fi - # # Set the default output mode if none was set. # -if [ $OUTPUT_MODE = none ] +if [ $OUTPUT_MODE = none -a $OUTPUT_MODE != skip ] then if [ $USE_IO = yes ] then @@ -65,7 +72,7 @@ fi # # If OUTPUT_MODE = none then something is wrong with setup and we shouldn't try to enable output. # -if [ $OUTPUT_MODE != none ] +if [ $OUTPUT_MODE != none -a $OUTPUT_MODE != skip ] then if [ $OUTPUT_MODE = hil -o $OUTPUT_MODE = sim ] @@ -76,15 +83,6 @@ then fi fi - if [ $OUTPUT_MODE = $OUTPUT_CMD ] - then - if ! $OUTPUT_CMD mode_$FMU_MODE - then - echo "$OUTPUT_CMD start failed" - tune_control play error - fi - fi - if [ $OUTPUT_MODE = uavcan_esc ] then if param compare UAVCAN_ENABLE 0 @@ -93,19 +91,30 @@ then fi fi - if [ $OUTPUT_MODE = io ] - then - . ${R}etc/init.d/rc.io - fi - # - # Start IO for RC input if needed. + # Start IO for PWM output or RC input if needed. # if [ $IO_PRESENT = yes ] then - if [ $OUTPUT_MODE != io ] + if ! px4io start + then + echo "PX4IO start failed" + tune_control play -t 18 # PROG_PX4IO_ERR + fi + fi + + if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ] + then + if param compare SYS_CTRL_ALLOC 1 then - . ${R}etc/init.d/rc.io + pwm_out start + dshot start + else + if ! $OUTPUT_CMD start + then + echo "$OUTPUT_CMD start failed" + tune_control play error + fi fi fi fi @@ -161,7 +170,7 @@ else fi fi -if [ $MIXER_AUX != none -a $AUX_MODE != none ] +if [ $MIXER_AUX != none ] then # # Load aux mixer. @@ -179,34 +188,26 @@ then if [ $MIXER_AUX_FILE != none ] then - # Start the output module - if $OUTPUT_CMD mode_${AUX_MODE} + # Append aux mixer to main device. + if param greater SYS_HITL 0 then - # Append aux mixer to main device. - if param greater SYS_HITL 0 + if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE} then - if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE} - then - echo "INFO [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}" - else - echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}" - fi + echo "INFO [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}" + else + echo "ERROR [init] Failed appending mixer: ${MIXER_AUX_FILE}" fi - if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ] + fi + if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ] + then + if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE} then - if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE} - then - echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}" - else - echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}" - fi + echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}" else - echo "INFO [init] setting PWM_AUX_OUT none" - set PWM_AUX_OUT none + echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}" fi else - echo "ERROR: Could not start: pwm_out mode_pwm" - tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR + echo "INFO [init] setting PWM_AUX_OUT none" set PWM_AUX_OUT none fi @@ -226,42 +227,12 @@ then fi fi -if [ $MIXER_AUX != none -a $AUX_MODE = none -a -e $OUTPUT_AUX_DEV ] -then - # - # Load aux mixer. - # - if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix ] - then - set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix - else - - if [ -f /etc/mixers/${MIXER_AUX}.aux.mix ] - then - set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.aux.mix - fi - fi - - if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE} - then - echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}" - - # Set PWM_AUX output frequency. - if [ $PWM_AUX_RATE != none ] - then - pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV} - fi - else - echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}" - fi -fi - if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ] then - if [ $PWM_OUT != none ] + if [ $PWM_OUT != none -a $PWM_MAIN_RATE != none ] then # Set PWM output frequency. - if [ $PWM_MAIN_RATE != none ] + if ! param compare SYS_CTRL_ALLOC 1 then pwm rate -c ${PWM_OUT} -r ${PWM_MAIN_RATE} fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io deleted file mode 100644 index 132b2fd51319..000000000000 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ /dev/null @@ -1,23 +0,0 @@ -#!/bin/sh -# -# PX4IO interface init script. -# - -# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs, -# instead, pwm_out_sim will publish that uORB -if [ $OUTPUT_MODE = hil ] -then - set HIL_ARG $OUTPUT_MODE -fi - -if [ $IO_PRESENT = yes ] -then - if px4io start $HIL_ARG - then - # Allow PX4IO to recover from midair restarts. - px4io recovery - else - echo "PX4IO start failed" - tune_control play -t 18 # PROG_PX4IO_ERR - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 0e81e7eda2b5..60d9e6532743 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -48,6 +48,14 @@ fi # End Estimator Group Selection # ############################################################################### +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi + # # Start Multicopter Rate Controller. # @@ -58,6 +66,11 @@ mc_rate_control start # mc_att_control start +if param greater -s MC_AT_EN 0 +then + mc_autotune_attitude_control start +fi + # # Start Multicopter Position Controller. # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 7a9fc5641437..d057bf3050c9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -7,7 +7,13 @@ set VEHICLE_TYPE mc -param set-default IMU_GYRO_RATEMAX 800 +# MAV_TYPE_QUADROTOR 2 +param set-default MAV_TYPE 2 + +if param compare IMU_GYRO_RATEMAX 400 +then + param set-default IMU_GYRO_RATEMAX 800 +fi param set-default NAV_ACC_RAD 2 @@ -25,4 +31,8 @@ param set-default GPS_UBX_DYNMODEL 6 # set MIXER_AUX pass +set MIXER quad_x + +set PWM_OUT 1234 + set PWM_AUX_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index 87a77281e3fd..d1e389627e90 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -12,6 +12,13 @@ ekf2 start & #attitude_estimator_q start #local_position_estimator start +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi # # Start attitude controllers. diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index 26db6d098c42..d7e826c49c40 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -7,13 +7,15 @@ set VEHICLE_TYPE rover +# MAV_TYPE_GROUND_ROVER 10 +param set-default MAV_TYPE 10 + # # Default parameters for UGVs. # param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 -param set-default NAV_DLL_ACT 0 param set-default NAV_ACC_RAD 2 param set-default NAV_LOITER_RAD 2 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8247025f8d9c..2694eb3c6412 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -4,19 +4,6 @@ # # NOTE: Script variables are declared/initialized/unset in the rcS script. # - -if ! ver hwcmp OMNIBUS_F4SD -then - if ! ver hwcmp BITCRAZE_CRAZYFLIE - then - # Configure all I2C buses to 100 KHz as they - # are all external or slow - # TODO: move this - pwm_out i2c 1 100000 - pwm_out i2c 2 100000 - fi -fi - ############################################################################### # Begin Optional drivers # ############################################################################### @@ -26,6 +13,12 @@ then batt_smbus start -X fi +# Start batmon driver if enabled using BATMON_DRIVER_EN +if param compare -s BATMON_DRIVER_EN 1 +then + batmon start -X #start on external bus +fi + # Sensors on the PWM interface bank if param compare -s SENS_EN_LL40LS 1 then @@ -90,18 +83,24 @@ then teraranger start -X fi -# Possible external pmw3901 optical flow sensor -if param greater -s SENS_EN_PMW3901 0 +# paa3905 optical flow sensor (external SPI) +if param greater -s SENS_EN_PAA3905 0 then - pmw3901 -S start + paa3905 -S start fi -# Possible external paw3902 optical flow sensor +# paw3902 optical flow sensor (external SPI) if param greater -s SENS_EN_PAW3902 0 then paw3902 -S start fi +# pmw3901 optical flow sensor (external SPI) +if param greater -s SENS_EN_PMW3901 0 +then + pmw3901 -S start +fi + # vl53l1x i2c distance sensor if param compare -s SENS_EN_VL53L1X 1 then @@ -111,14 +110,83 @@ fi # ADIS16448 spi external IMU if param compare -s SENS_EN_ADIS164X 1 then - adis16448 -S start + if param compare -s SENS_OR_ADIS164X 0 + then + adis16448 -S start + fi + if param compare -s SENS_OR_ADIS164X 4 + then + adis16448 -S start -R 4 + fi +fi + +# Eagle Tree airspeed sensor external I2C +if param compare -s SENS_EN_ETSASPD 1 +then + ets_airspeed start -X +fi + +# Sensirion SDP3X differential pressure sensor external I2C +if param compare -s SENS_EN_SDP3X 1 +then + if ! sdp3x start -X + then + # try another common address + sdp3x start -X -a 0x22 + fi +fi + +# TE MS4515 differential pressure sensor external I2C +if param compare -s SENS_EN_MS4515 1 +then + ms4515 start -X +fi + +# TE MS4525DO differential pressure sensor external I2C +if param compare -s SENS_EN_MS4525DO 1 +then + ms4525do start -X +fi + +# TE MS5525DSO differential pressure sensor external I2C +if param compare -s SENS_EN_MS5525DS 1 +then + ms5525dso start -X +fi + +# SHT3x temperature and hygrometer sensor, external I2C +if param compare -s SENS_EN_SHT3X 1 +then + sht3x start -X + sht3x start -X -a 0x45 +fi + +# IR-LOCK sensor external I2C +if param compare -s SENS_EN_IRLOCK 1 +then + irlock start -X +fi + +# SPL06 sensor external I2C +if param compare -s SENS_EN_SPL06 1 +then + spl06 -X start + spl06 -X -a 0x77 start +fi + +# PCF8583 counter (RPM sensor) +if param compare -s SENS_EN_PCF8583 1 +then + pcf8583 start -X + pcf8583 start -X -a 0x51 fi # probe for optional external I2C devices if param compare SENS_EXT_I2C_PRB 1 then + icm20948_i2c_passthrough -X -q start + # compasses - ak09916 -X -R 6 -q start # external AK09916 (Here2) is rotated 270 degrees yaw hmc5883 -T -X -q start ist8308 -X -q start ist8310 -X -q start @@ -127,33 +195,6 @@ then qmc5883l -X -q start rm3100 -X -q start - # differential pressure sensors - if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ] - then - # Always try to start the airspeed sensors - # even if their usage might be disabled - sdp3x_airspeed start -X -q - sdp3x_airspeed start -X -a 0x22 -q - - # Pixhawk 2.1 has a MS5611 on I2C which gets wrongly - # detected as MS5525 because the chip manufacturer was so - # clever to assign the same I2C address and skip a WHO_AM_I - # register. - if [ $BOARD_FMUV3 = 21 ] - then - ms5525_airspeed start -X -b 2 -q - else - ms5525_airspeed start -X -q - fi - - ms4525_airspeed start -X -q - - ets_airspeed start -X -q - fi + # start last (wait for possible icm20948 passthrough mode) + ak09916 -X -q start fi - -############################################################################### -# End Optional drivers # -############################################################################### - -sensors start diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps index 1e4074b7272d..1855805fc22a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -15,6 +15,14 @@ ekf2 start & # End Estimator Group Selection # ############################################################################### +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi + # # Start UUV Attitude Controller. # diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults index 6d525fd0e1e2..01e755fc290e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_defaults @@ -7,6 +7,9 @@ set VEHICLE_TYPE uuv +# MAV_TYPE_SUBMARINE 12 +param set-default MAV_TYPE 12 + param set-default PWM_MAIN_MAX 1950 param set-default PWM_MAIN_MIN 1050 param set-default PWM_MAIN_DISARM 1500 diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index e9f01eae5d50..aac1b76fed9f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -12,19 +12,9 @@ if [ $VEHICLE_TYPE = fw ] then if [ $MIXER = none ] then - # Set default mixer for fixed wing if not defined. - set MIXER AERT + echo "FW mixer undefined" fi - if [ $MAV_TYPE = none ] - then - # Set a default MAV_TYPE = 1 if not defined. - set MAV_TYPE 1 - fi - - # Set the mav type parameter. - param set MAV_TYPE ${MAV_TYPE} - # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface @@ -42,41 +32,6 @@ then echo "MC mixer undefined" fi - if [ $MAV_TYPE = none ] - then - # Set a default MAV_TYPE = 2 if not defined. - set MAV_TYPE 2 - - # Use mixer to detect vehicle type - if [ $MIXER = coax ] - then - set MAV_TYPE 3 - fi - if [ $MIXER = hexa_x -o $MIXER = hexa_+ ] - then - set MAV_TYPE 13 - fi - if [ $MIXER = hexa_cox ] - then - set MAV_TYPE 13 - fi - if [ $MIXER = octo_x -o $MIXER = octo_+ ] - then - set MAV_TYPE 14 - fi - if [ $MIXER = octo_cox -o $MIXER = octo_cox_w ] - then - set MAV_TYPE 14 - fi - if [ $MIXER = tri_y_yaw- -o $MIXER = tri_y_yaw+ ] - then - set MAV_TYPE 15 - fi - fi - - # Set the mav type parameter. - param set MAV_TYPE ${MAV_TYPE} - # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface @@ -91,19 +46,9 @@ if [ $VEHICLE_TYPE = rover ] then if [ $MIXER = none ] then - # Set default mixer for UGV if not defined. - set MIXER rover_generic - fi - - if [ $MAV_TYPE = none ] - then - # Set a default MAV_TYPE = 10 if not defined. - set MAV_TYPE 10 + echo "rover mixer undefined" fi - # Set the mav type parameter. - param set MAV_TYPE ${MAV_TYPE} - # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface @@ -121,25 +66,6 @@ then echo "VTOL mixer undefined" fi - if [ $MAV_TYPE = none ] - then - # Set a default MAV_TYPE = 19 if not defined. - set MAV_TYPE 19 - - # Use mixer to detect vehicle type. - if [ $MIXER = firefly6 ] - then - set MAV_TYPE 21 - fi - if [ $MIXER = quad_x_pusher_vtol ] - then - set MAV_TYPE 22 - fi - fi - - # Set the mav type parameter. - param set MAV_TYPE ${MAV_TYPE} - # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface @@ -157,15 +83,6 @@ then echo "Airship mixer undefined" fi - if [ $MAV_TYPE = none ] - then - # Set a default MAV_TYPE = 7 if not defined. - set MAV_TYPE 7 - fi - - # Set the mav type parameter. - param set MAV_TYPE ${MAV_TYPE} - # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface @@ -183,19 +100,11 @@ then echo "UUV mixer undefined" fi - if [ $MAV_TYPE = none ] - then - # Set default MAV_TYPE to submarine if not defined - set MAV_TYPE 12 - fi - - param set MAV_TYPE ${MAV_TYPE} # Load mixer and configure outputs. . ${R}etc/init.d/rc.interface # Start standard vtol apps. . ${R}etc/init.d/rc.uuv_apps - fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index c6c1cacfbfbe..afcf32f45a5e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -15,6 +15,14 @@ ekf2 start & # End Estimator group selection # ############################################################################### +if param compare SYS_CTRL_ALLOC 1 +then + # + # Start Control Allocator + # + control_allocator start +fi + airspeed_selector start vtol_att_control start @@ -25,8 +33,14 @@ flight_mode_manager start mc_pos_control start vtol mc_hover_thrust_estimator start +if param greater -s MC_AT_EN 0 +then + mc_autotune_attitude_control start +fi + fw_att_control start vtol fw_pos_control_l1 start vtol +fw_autotune_attitude_control start vtol # Start Land Detector # Multicopter for now until we have something for VTOL diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index fd584941e685..5440ee5a70e1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -7,12 +7,17 @@ set VEHICLE_TYPE vtol +# MAV_TYPE_VTOL_FIXEDROTOR 22 +param set-default MAV_TYPE 22 + param set-default MIS_TAKEOFF_ALT 20 param set-default MIS_YAW_TMT 10 param set-default EKF2_ARSP_THR 10 param set-default EKF2_FUSE_BETA 1 +param set-default HTE_VXY_THR 2.0 + param set-default MIS_DIST_WPS 5000 param set-default MPC_ACC_HOR_MAX 2 @@ -23,6 +28,7 @@ param set-default MPC_XY_ERR_MAX 5 param set-default MPC_XY_VEL_MAX 4 param set-default MPC_Z_VEL_MAX_DN 1.5 param set-default MPC_JERK_MAX 4.5 +param set-default MPC_YAW_MODE 4 param set-default NAV_ACC_RAD 3 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 38af11804b81..c0b9ce7f7602 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -21,23 +21,19 @@ set +e # it wastes flash # set R / -set AUTOCNF no -set AUX_MODE pwm set FCONFIG /fs/microsd/etc/config.txt set FEXTRAS /fs/microsd/etc/extras.txt -set FMU_MODE pwm set FRC /fs/microsd/etc/rc.txt set IOFW "/etc/extras/px4_io-v2_default.bin" set IO_PRESENT no set LOGGER_ARGS "" -set LOGGER_BUF 14 -set MAV_TYPE none +set LOGGER_BUF 8 set MIXER none set MIXER_AUX none set MIXER_FILE none set MIXER_EXTRA none set OUTPUT_MODE none -set PARAM_FILE /fs/microsd/params +set PARAM_FILE "" set PWM_OUT none set PWM_MAIN_RATE p:PWM_MAIN_RATE set PWM_AUX_OUT none @@ -46,21 +42,14 @@ set PWM_EXTRA_OUT none set PWM_EXTRA_RATE p:PWM_EXTRA_RATE set EXTRA_MIXER_MODE none set RC_INPUT_ARGS "" +set SDCARD_AVAILABLE no +set SDCARD_EXT_PATH /fs/microsd/ext_autostart +set SDCARD_FORMAT no set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers set STARTUP_TUNE 1 set USE_IO no set VEHICLE_TYPE none -# -# Mount the procfs. -# -mount -t procfs /proc - -# -# Start CDC/ACM serial driver. -# -sercon - # # Print full system version. # @@ -69,58 +58,56 @@ ver all # # Try to mount the microSD card. # -# REBOOTWORK this needs to start after the flight control loop. -if mount -t vfat /dev/mmcsd0 /fs/microsd +if [ -b "/dev/mmcsd0" ] then - if hardfault_log check + if mount -t vfat /dev/mmcsd0 /fs/microsd then - # Error tune. - set STARTUP_TUNE 2 - if hardfault_log commit + if [ -f "/fs/microsd/.format" ] then - hardfault_log reset - fi - fi - - # Prevent MacOS and Ubuntu from creating unnecessary temporary files on the microSD card + echo "INFO [init] format /dev/mmcsd0 requested (/fs/microsd/.format)" + set SDCARD_FORMAT yes + rm /fs/microsd/.format + umount /fs/microsd - # block MacOS Spotlight indexing (.Spotlight-V100 folder) - if [ ! -f "/fs/microsd/.metadata_never_index" ]; then - cat > /fs/microsd/.metadata_never_index - fi - - # block MacOS trashes - if [ ! -f "/fs/microsd/.Trashes" ]; then - cat > /fs/microsd/.Trashes + else + set SDCARD_AVAILABLE yes + fi fi - # block MacOS logging of filesystem events - if [ ! -d "/fs/microsd/.fseventsd" ]; then - mkdir /fs/microsd/.fseventsd - fi + if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ] + then + echo "INFO [init] formatting /dev/mmcsd0" + set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds) - if [ ! -f "/fs/microsd/.fseventsd/no_log" ]; then - cat > /fs/microsd/.fseventsd/no_log - fi + if mkfatfs -F 32 /dev/mmcsd0 + then + echo "INFO [init] card formatted" - # block Ubuntu trash - if [ ! -f "/fs/microsd/.Trash-1000" ]; then - cat > /fs/microsd/.Trash-1000 + if mount -t vfat /dev/mmcsd0 /fs/microsd + then + set SDCARD_AVAILABLE yes + set STARTUP_TUNE 14 # tune 14 = SD_INIT + else + echo "ERROR [init] card mount failed" + fi + else + echo "ERROR [init] format failed" + fi fi -else - # tune SD_INIT - set STARTUP_TUNE 14 # tune 14 = SD_INIT - if mkfatfs /dev/mmcsd0 + if [ $SDCARD_AVAILABLE = yes ] then - if mount -t vfat /dev/mmcsd0 /fs/microsd + if hardfault_log check then - echo "INFO [init] card formatted" - else - set STARTUP_TUNE 15 # tune 15 = SD_ERROR - echo "ERROR [init] format failed" + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if hardfault_log commit + then + hardfault_log reset + fi fi fi + + set PARAM_FILE /fs/microsd/params fi # @@ -153,21 +140,48 @@ else param select $PARAM_FILE if ! param import then - param reset_all + echo "ERROR [init] param import failed" + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + + param dump $PARAM_FILE + + if [ -d "/fs/microsd" ] + then + dmesg >> /fs/microsd/param_import_fail.txt & + + # try to make a backup copy + cp $PARAM_FILE /fs/microsd/param_import_fail.bson & + fi + + # try importing from backup file + if [ -f "/fs/microsd/parameters_backup.bson" ] + then + echo "[init] importing from parameter backup" + + # dump current backup file contents for comparison + param dump /fs/microsd/parameters_backup.bson + + param import /fs/microsd/parameters_backup.bson + fi fi - if ver hwtypecmp V5X00 V5X90 V5Xa0 + + if [ $SDCARD_AVAILABLE = yes ] + then + param select-backup /fs/microsd/parameters_backup.bson + fi + + if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X then netman update -i eth0 fi # - # Set AUTOCNF flag to use it in AUTOSTART scripts. + # If the airframe has been previously reset SYS_AUTCONFIG will have been set to 1 and other params will be reset on the next boot. # if param greater SYS_AUTOCONFIG 0 then - # Wipe out params except RC*, flight modes, total flight time, calibration parameters, next flight UUID - param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT* - set AUTOCNF yes + # Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID. + param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT* fi # @@ -198,20 +212,20 @@ else # tone_alarm start - # - # Play the startup tune (if not disabled or there is an error) - # - param compare CBRK_BUZZER 782090 - if [ $? != 0 -o $STARTUP_TUNE != 1 ] - then - tune_control play -t $STARTUP_TUNE - fi - # # Waypoint storage. # REBOOTWORK this needs to start in parallel. # - dataman start + if param compare SYS_DM_BACKEND 1 + then + dataman start -r + else + if param compare SYS_DM_BACKEND 0 + then + # dataman start default + dataman start + fi + fi # # Start the socket communication send_event handler. @@ -232,10 +246,22 @@ else # # Set parameters and env variables for selected AUTOSTART. # + set AUTOSTART_PATH etc/init.d/rc.autostart if ! param compare SYS_AUTOSTART 0 then - . ${R}etc/init.d/rc.autostart + if param greater SYS_AUTOSTART 1000000 + then + # Use external startup file + if [ $SDCARD_AVAILABLE = yes ] + then + set AUTOSTART_PATH etc/init.d/rc.autostart_ext + else + echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS" + fi + fi + . ${R}$AUTOSTART_PATH fi + unset AUTOSTART_PATH # # Override parameters from user configuration file. @@ -247,11 +273,25 @@ else fi # - # If autoconfig parameter was set, reset it and save parameters. + # Check if UAVCAN is enabled, default to it for ESCs. # - if [ $AUTOCNF = yes ] + if param greater -s UAVCAN_ENABLE 0 then - param set SYS_AUTOCONFIG 0 + # Start core UAVCAN module. + if uavcan start + then + if param greater UAVCAN_ENABLE 2 + then + set OUTPUT_MODE uavcan_esc + fi + else + tune_control play error + fi + else + if param greater -s CYPHAL_ENABLE 0 + then + cyphal start + fi fi # @@ -285,6 +325,8 @@ else else tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi + else + tune_control stop fi fi fi @@ -301,7 +343,7 @@ else if [ $USE_IO = yes -a $IO_PRESENT = no ] then echo "PX4IO not found" - tune_control play error + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE fi # @@ -309,6 +351,7 @@ else # start before commander # rc_update start + manual_control start # # Sensors System (start before Commander so Preflight checks are properly run). @@ -352,85 +395,36 @@ else battery_status start fi + sensors start commander start fi - # Sensors on the PWM interface bank. - if param compare -s SENS_EN_LL40LS 1 - then - # Clear pins 5 and 6. - set FMU_MODE pwm4 - set AUX_MODE pwm4 - fi - + # + # Configure vehicle type specific parameters. + # Note: rc.vehicle_setup is the entry point for rc.interface, + # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. + # + . ${R}etc/init.d/rc.vehicle_setup - # Check if ATS is enabled - if param compare FD_EXT_ATS_EN 1 + # Pre-takeoff continuous magnetometer calibration + if param compare -s MBE_ENABLE 1 then - # Clear pins 5 and 6. - set FMU_MODE pwm4 - set AUX_MODE pwm4 + mag_bias_estimator start fi if param greater -s TRIG_MODE 0 then - # We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output. - if param compare TRIG_PINS 56 - then - # clear pins 5 and 6 - set FMU_MODE pwm4 - set AUX_MODE pwm4 - else - if param compare TRIG_PINS 78 - then - # clear pins 7 and 8 - set FMU_MODE pwm6 - set AUX_MODE pwm6 - else - set FMU_MODE none - set AUX_MODE none - fi - fi - camera_trigger start camera_feedback start fi - # - # Check if UAVCAN is enabled, default to it for ESCs. - # - if param greater -s UAVCAN_ENABLE 0 - then - # Start core UAVCAN module. - if uavcan start - then - if param greater UAVCAN_ENABLE 1 - then - # Start UAVCAN firmware update server and dynamic node ID allocation server. - uavcan start fw - tune_control play -t 1 - if param greater UAVCAN_ENABLE 2 - then - set OUTPUT_MODE uavcan_esc - fi - fi - else - tune_control play error - fi - fi - - if param greater -s UAVCAN_V1_ENABLE 0 - then - uavcan_v1 start - fi - # # Optional board mavlink streams: rc.board_mavlink # set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink if [ -f $BOARD_RC_MAVLINK ] then - echo "Board extras: ${BOARD_RC_MAVLINK}" + echo "Board mavlink: ${BOARD_RC_MAVLINK}" . $BOARD_RC_MAVLINK fi unset BOARD_RC_MAVLINK @@ -447,14 +441,13 @@ else rc_input start $RC_INPUT_ARGS fi - # - # Configure vehicle type specific parameters. - # Note: rc.vehicle_setup is the entry point for rc.interface, - # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. - # - . ${R}etc/init.d/rc.vehicle_setup + # PPS capture driver (before pwm_out) + if param greater -s PPS_CAP_ENABLE 0 + then + pps_capture start + fi - # Camera capture driver + # Camera capture driver (before pwm_out) if param greater -s CAM_CAP_FBACK 0 then if camera_capture start @@ -463,6 +456,15 @@ else fi fi + # + # Play the startup tune (if not disabled or there is an error) + # + param compare CBRK_BUZZER 782090 + if [ "$?" != "0" -o "$STARTUP_TUNE" != "1" ] + then + tune_control play -t $STARTUP_TUNE + fi + # # Start the navigator. # @@ -474,17 +476,11 @@ else . ${R}etc/init.d/rc.thermal_cal # - # Start vmount to control mounts such as gimbals, disabled by default. + # Start gimbal to control mounts such as gimbals, disabled by default. # if param greater -s MNT_MODE_IN -1 then - vmount start - fi - - # Check for flow sensor - if param compare -s SENS_EN_PX4FLOW 1 - then - px4flow start -X + gimbal start fi # Blacksheep telemetry @@ -503,6 +499,12 @@ else gyro_calibration start fi + # Check for px4flow sensor + if param compare -s SENS_EN_PX4FLOW 1 + then + px4flow start -X & + fi + # # Optional board supplied extras: rc.board_extras # @@ -536,21 +538,13 @@ else . ${R}etc/init.d/rc.autostart.post fi - # - # Bootloader upgrade - # - if param compare -s SYS_BL_UPDATE 1 + + set BOARD_BOOTLOADER_UPGRADE ${R}etc/init.d/rc.board_bootloader_upgrade + if [ -f $BOARD_BOOTLOADER_UPGRADE ] then - if [ -f "/etc/extras/bootloader.bin" ] - then - param set SYS_BL_UPDATE 0 - param save - echo "bootloader update..." - bl_update "/etc/extras/bootloader.bin" - echo "bootloader update done, rebooting" - reboot - fi + sh $BOARD_BOOTLOADER_UPGRADE fi + unset BOARD_BOOTLOADER_UPGRADE # # End of autostart. @@ -561,17 +555,13 @@ fi # Unset all script parameters to free RAM. # unset R -unset AUTOCNF -unset AUX_MODE unset FCONFIG unset FEXTRAS -unset FMU_MODE unset FRC unset IO_PRESENT unset IOFW unset LOGGER_ARGS unset LOGGER_BUF -unset MAV_TYPE unset MIXER unset MIXER_AUX unset MIXER_FILE @@ -584,6 +574,9 @@ unset PWM_OUT unset PWM_EXTRA_OUT unset PWM_EXTRA_RATE unset RC_INPUT_ARGS +unset SDCARD_AVAILABLE +unset SDCARD_EXT_PATH +unset SDCARD_FORMAT unset SDCARD_MIXERS_PATH unset STARTUP_TUNE unset USE_IO diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt index eaab17db6daa..60f6d12ff2ef 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt @@ -33,16 +33,11 @@ px4_add_romfs_files( autogyro_sitl.main.mix - boat_sitl.main.mix delta_wing_sitl.main.mix package_drop.aux.mix plane_sitl.main.mix quad_x_vtol.main.mix - rover_ackermann_sitl.main.mix - rover_diff_sitl.main.mix standard_vtol_sitl.main.mix tiltrotor_sitl.main.mix uuv_x_sitl.main.mix - vectored6dof_sitl.main.mix - tiltrotor_sitl_direct.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers-sitl/boat_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/boat_sitl.main.mix deleted file mode 100644 index 49cfa1ba2076..000000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/boat_sitl.main.mix +++ /dev/null @@ -1,15 +0,0 @@ -Mixer for SITL boat -========================================================= - -Throttle of left propeller of boat on Output 0 ---------------------------------------- -M: 2 -S: 0 2 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - - -Throttle of right propeller of boat on Output 1 ---------------------------------------- -M: 2 -S: 0 2 -10000 -10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix index 5fd6115f46b9..836bd0786ede 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix +++ b/ROMFS/px4fmu_common/mixers-sitl/plane_sitl.main.mix @@ -21,14 +21,16 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 # mixer for the left aileron -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 -10000 -10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 # mixer for the right aileron -M: 1 +M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 # mixer for the elevator M: 1 diff --git a/ROMFS/px4fmu_common/mixers-sitl/rover_ackermann_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/rover_ackermann_sitl.main.mix deleted file mode 100644 index d56d5a067d9e..000000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/rover_ackermann_sitl.main.mix +++ /dev/null @@ -1,46 +0,0 @@ -Mixer for SITL rover -========================================================= - -Output 0 ---------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -Output 1 ---------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 2 10000 10000 0 -10000 10000 - -Output 2 ---------------------------------------- -Z: - -Output 3 ---------------------------------------- -Z: - -Output 4 ---------------------------------------- -Z: - -Output 5 ---------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 6 ---------------------------------------- -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 7 ---------------------------------------- -Z: - -Output 8 ---------------------------------------- -Z: diff --git a/ROMFS/px4fmu_common/mixers-sitl/rover_diff_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/rover_diff_sitl.main.mix deleted file mode 100644 index c5f378410fda..000000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/rover_diff_sitl.main.mix +++ /dev/null @@ -1,46 +0,0 @@ -Mixer for SITL rover -========================================================= - -Output 0 ---------------------------------------- -M: 2 -S: 0 2 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 1 ---------------------------------------- -M: 2 -S: 0 2 10000 10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 2 ---------------------------------------- -Z: - -Output 3 ---------------------------------------- -Z: - -Output 4 ---------------------------------------- -Z: - -Output 5 ---------------------------------------- -M: 2 -S: 0 2 -10000 -10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 6 ---------------------------------------- -M: 2 -S: 0 2 -10000 -10000 0 -10000 10000 -S: 0 3 10000 10000 0 -10000 10000 - -Output 7 ---------------------------------------- -Z: - -Output 8 ---------------------------------------- -Z: diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl.main.mix index f57230b4a8e6..54fd3c51f270 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl.main.mix +++ b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl.main.mix @@ -8,22 +8,22 @@ R: 4x # tilt servo motor 1 M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # tilt servo motor 2 M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # tilt servo motor 3 M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # tilt servo motor 4 M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # mixer for the left aileron M: 1 diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix deleted file mode 100644 index ea3adaa0b77d..000000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_sitl_direct.main.mix +++ /dev/null @@ -1,14 +0,0 @@ -Mixer for quad tiltrotor (x motor configuration) -================================================ - -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 -A: 6 -A: 7 -A: 8 -A: 9 -A: 10 diff --git a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix deleted file mode 100644 index 1b27ef16404c..000000000000 --- a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix +++ /dev/null @@ -1,40 +0,0 @@ -# Motor 1 -M: 3 -S: 0 2 -4000 -4000 0 -4000 +4000 -S: 0 3 +4000 +4000 0 -4000 +4000 -S: 0 4 -4000 -4000 0 -4000 +4000 -# Motor 2 -M: 3 -S: 0 2 +4000 +4000 0 -4000 +4000 -S: 0 3 +4000 +4000 0 -4000 +4000 -S: 0 4 +4000 +4000 0 -4000 +4000 -# Motor 3 -M: 3 -S: 0 2 -4000 -4000 0 -4000 +4000 -S: 0 3 +4000 +4000 0 -4000 +4000 -S: 0 4 +4000 +4000 0 -4000 +4000 -# Motor 4 -M: 3 -S: 0 2 +4000 +4000 0 -4000 +4000 -S: 0 3 +4000 +4000 0 -4000 +4000 -S: 0 4 -4000 -4000 0 -4000 +4000 -# Motor 5 -M: 3 -S: 0 0 -4000 -4000 0 -4000 +4000 -S: 0 1 +4000 +4000 0 -4000 +4000 -S: 0 5 -4000 -4000 0 -4000 +4000 -# Motor 6 -M: 3 -S: 0 0 -4000 -4000 0 -4000 +4000 -S: 0 1 -4000 -4000 0 -4000 +4000 -S: 0 5 +4000 +4000 0 -4000 +4000 -# Motor 7 -M: 3 -S: 0 0 +4000 +4000 0 -4000 +4000 -S: 0 1 +4000 +4000 0 -4000 +4000 -S: 0 5 +4000 +4000 0 -4000 +4000 -# Motor 8 -M: 3 -S: 0 0 +4000 +4000 0 -4000 +4000 -S: 0 1 -4000 -4000 0 -4000 +4000 -S: 0 5 -4000 -4000 0 -4000 +4000 diff --git a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix index 18d8ed802f40..8bd3fd0fa1ce 100644 --- a/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAERTWF.main.mix @@ -10,7 +10,7 @@ to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -20,11 +20,11 @@ endpoints to suit. M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 Elevator mixer ------------ diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix index 17a2c0bbd63e..07070c753599 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF.main.mix @@ -11,7 +11,7 @@ to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,11 +21,11 @@ endpoints to suit. M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix b/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix index 6bd4568da8c3..4baa82a89d8e 100644 --- a/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix +++ b/ROMFS/px4fmu_common/mixers/AAVVTWFF_vtail.main.mix @@ -9,20 +9,20 @@ output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw), 3 (thrust) 4 (flaps), 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up mechanically reversed. M: 2 S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 S: 0 0 -10000 -10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt index df861065a271..00903468fc83 100644 --- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt +++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt @@ -39,21 +39,16 @@ px4_add_romfs_files( AETRFG.main.mix babyshark.main.mix blade130.main.mix - caipi.main.mix CCPM.main.mix - claire.aux.mix - claire.main.mix cloudship.main.mix coax.main.mix delta.main.mix deltaquad.main.mix - direct.main.mix dodeca_bottom_cox.aux.mix dodeca_top_cox.main.mix firefly6.aux.mix firefly6.main.mix fw_generic_wing.main.mix - FX79.main.mix generic_diff_rover.main.mix hexa_cox.main.mix hexa_+.main.mix @@ -66,7 +61,6 @@ px4_add_romfs_files( octo_+.main.mix octo_x.main.mix pass.aux.mix - phantom.main.mix quad_dc.main.mix quad_h.main.mix quad_+.main.mix @@ -87,12 +81,11 @@ px4_add_romfs_files( tri_y_yaw-.main.mix uuv_x.main.mix vectored6dof.main.mix - Viper.main.mix vtol_AAERT.aux.mix vtol_AAVVT.aux.mix vtol_TTTTAAER.aux.mix vtol_convergence.main.mix vtol_delta.aux.mix vtol_tailsitter_duo.main.mix - wingwing.main.mix + vtol_tailsitter_duo_sat.main.mix ) diff --git a/ROMFS/px4fmu_common/mixers/FX79.main.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix deleted file mode 100644 index a1e0e345cc3f..000000000000 --- a/ROMFS/px4fmu_common/mixers/FX79.main.mix +++ /dev/null @@ -1,50 +0,0 @@ -FX-79 Delta-wing mixer for PX4FMU -================================= - -Designed for FX-79. - -TODO (sjwilks): Add mixers for flaps. - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -S: 0 0 8500 8500 0 -10000 10000 -S: 0 1 9500 9500 0 -10000 10000 - -M: 2 -S: 0 0 8500 8500 0 -10000 10000 -S: 0 1 -9500 -9500 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix b/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix index bed0fedee29b..23c146904bbd 100644 --- a/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix +++ b/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix @@ -1,5 +1,6 @@ Aileron/rudder/elevator/throttle mixer for PX4FMU ================================================== +# @board px4_fmu-v2 exclude Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch) and 3 (thrust). diff --git a/ROMFS/px4fmu_common/mixers/TF-G2.main.mix b/ROMFS/px4fmu_common/mixers/TF-G2.main.mix index 9aea7cef50b8..37acba1b5542 100644 --- a/ROMFS/px4fmu_common/mixers/TF-G2.main.mix +++ b/ROMFS/px4fmu_common/mixers/TF-G2.main.mix @@ -1,5 +1,6 @@ TF-G2 autogyro mixer ================================================== +# @board px4_fmu-v2 exclude Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch) and 3 (thrust). diff --git a/ROMFS/px4fmu_common/mixers/Viper.main.mix b/ROMFS/px4fmu_common/mixers/Viper.main.mix deleted file mode 100644 index adc0e966967a..000000000000 --- a/ROMFS/px4fmu_common/mixers/Viper.main.mix +++ /dev/null @@ -1,65 +0,0 @@ -Viper Delta-wing mixer -================================= - -Designed for Viper. - -TODO (sjwilks): Add mixers for flaps. - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 - -M: 2 -S: 0 0 7500 7500 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -S: 0 3 0 20000 -10000 -10000 10000 - -Inputs to the mixer come from channel group 2 (payload), channels 0 -(bay servo 1), 1 (bay servo 2) and 3 (drop release). ------------------------------------------------------ - -M: 1 -S: 2 0 10000 10000 0 -10000 10000 - -M: 1 -S: 2 1 10000 10000 0 -10000 10000 - -M: 1 -S: 2 2 -8000 -8000 0 -10000 10000 - - diff --git a/ROMFS/px4fmu_common/mixers/babyshark.main.mix b/ROMFS/px4fmu_common/mixers/babyshark.main.mix index 5ef55059c62e..7aae2aa4afe8 100644 --- a/ROMFS/px4fmu_common/mixers/babyshark.main.mix +++ b/ROMFS/px4fmu_common/mixers/babyshark.main.mix @@ -2,6 +2,7 @@ # 1-4 (main): Ailerons (Y-cable), A-tail left, Pusher, A-tail right # 5-8 (main): quad motors #============================= +# @board px4_fmu-v2 exclude Aileron mixer (roll) diff --git a/ROMFS/px4fmu_common/mixers/caipi.main.mix b/ROMFS/px4fmu_common/mixers/caipi.main.mix deleted file mode 100644 index 68eea39b5183..000000000000 --- a/ROMFS/px4fmu_common/mixers/caipi.main.mix +++ /dev/null @@ -1,48 +0,0 @@ -Delta-wing mixer -=========================== - -Designed for TBS Caipirinha - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -S: 0 0 8000 8000 0 -10000 10000 -S: 0 1 9000 9000 0 -10000 10000 - -M: 2 -S: 0 0 8000 8000 0 -10000 10000 -S: 0 1 -9000 -9000 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/claire.aux.mix b/ROMFS/px4fmu_common/mixers/claire.aux.mix deleted file mode 100644 index 7213daf2a29c..000000000000 --- a/ROMFS/px4fmu_common/mixers/claire.aux.mix +++ /dev/null @@ -1,28 +0,0 @@ -# mixer for the CruiseAder Claire tilt mechansim servo and elevons - -======================================================================= - - -Tilt mechanism servo mixer - ---------------------------- - -M: 1 - - -S: 1 4 10000 10000 0 -10000 10000 - - - -Elevon mixers - -------------- - -M: 2 - -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 7500 7500 0 -10000 10000 - -M: 2 -S: 1 0 7500 7500 0 -10000 10000 -S: 1 1 -7500 -7500 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/claire.main.mix b/ROMFS/px4fmu_common/mixers/claire.main.mix deleted file mode 100644 index af84d3d8a1a3..000000000000 --- a/ROMFS/px4fmu_common/mixers/claire.main.mix +++ /dev/null @@ -1,7 +0,0 @@ -# CruiseAder Claire Main Multirotor mixer for PX4FMU - -# - -#=========================== - -R: 4x diff --git a/ROMFS/px4fmu_common/mixers/cloudship.main.mix b/ROMFS/px4fmu_common/mixers/cloudship.main.mix index 5932dd1d9874..decee989ef06 100644 --- a/ROMFS/px4fmu_common/mixers/cloudship.main.mix +++ b/ROMFS/px4fmu_common/mixers/cloudship.main.mix @@ -1,6 +1,8 @@ Thrust tilt/ Starboard Thrust / Port Thrust / Tail Thrust mixer for PX4FMU ======================================================= +# @board px4_fmu-v2 exclude + This file defines mixers suitable for controlling an airship with a thrust tilt, starboard and port thruster and a tail thruster using PX4FMU. The configuration assumes the starboard thruster is connected to PX4FMU diff --git a/ROMFS/px4fmu_common/mixers/delta.main.mix b/ROMFS/px4fmu_common/mixers/delta.main.mix index 6188948ee71c..0059951abe24 100644 --- a/ROMFS/px4fmu_common/mixers/delta.main.mix +++ b/ROMFS/px4fmu_common/mixers/delta.main.mix @@ -1,5 +1,6 @@ Delta-wing mixer for PX4FMU =========================== +# @board px4_fmu-v2 exclude This file defines mixers suitable for controlling a delta wing aircraft using PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU diff --git a/ROMFS/px4fmu_common/mixers/deltaquad.main.mix b/ROMFS/px4fmu_common/mixers/deltaquad.main.mix index bef989d92ba4..11642e3601c8 100644 --- a/ROMFS/px4fmu_common/mixers/deltaquad.main.mix +++ b/ROMFS/px4fmu_common/mixers/deltaquad.main.mix @@ -1,5 +1,6 @@ # DeltaQuad mixer for PX4FMU #============================= +# @board px4_fmu-v2 exclude Quad motors 1 - 4 ------------- diff --git a/ROMFS/px4fmu_common/mixers/direct.main.mix b/ROMFS/px4fmu_common/mixers/direct.main.mix deleted file mode 100644 index 65194e94b71c..000000000000 --- a/ROMFS/px4fmu_common/mixers/direct.main.mix +++ /dev/null @@ -1,10 +0,0 @@ -# Direct mixer - -A: 0 -A: 1 -A: 2 -A: 3 -A: 4 -A: 5 -A: 6 -A: 7 diff --git a/ROMFS/px4fmu_common/mixers/dodeca_bottom_cox.aux.mix b/ROMFS/px4fmu_common/mixers/dodeca_bottom_cox.aux.mix index 3bcbdbc823fb..8429e71cba1a 100644 --- a/ROMFS/px4fmu_common/mixers/dodeca_bottom_cox.aux.mix +++ b/ROMFS/px4fmu_common/mixers/dodeca_bottom_cox.aux.mix @@ -1,3 +1,4 @@ # Dodeca Cox +# @board px4_fmu-v2 exclude R: 6a diff --git a/ROMFS/px4fmu_common/mixers/dodeca_top_cox.main.mix b/ROMFS/px4fmu_common/mixers/dodeca_top_cox.main.mix index b3049115e0ce..6250c3db0aa2 100644 --- a/ROMFS/px4fmu_common/mixers/dodeca_top_cox.main.mix +++ b/ROMFS/px4fmu_common/mixers/dodeca_top_cox.main.mix @@ -1,3 +1,4 @@ # Dodeca Cox +# @board px4_fmu-v2 exclude R: 6m diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index eab62e1f0c5b..9007a23b961a 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -1,10 +1,12 @@ # mixer for the FireFly6 tilt mechansim servo, elevons and landing gear ======================================================================= +# @board px4_fmu-v2 exclude + Tilt mechanism servo mixer --------------------------- M: 1 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 Elevon mixers ------------- diff --git a/ROMFS/px4fmu_common/mixers/firefly6.main.mix b/ROMFS/px4fmu_common/mixers/firefly6.main.mix index 18f364390c30..59148528c6a7 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.main.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.main.mix @@ -1,4 +1,6 @@ # FireFly6 mixer for PX4FMU # #=========================== +# @board px4_fmu-v2 exclude + R: 6c diff --git a/ROMFS/px4fmu_common/mixers/mount_legs.aux.mix b/ROMFS/px4fmu_common/mixers/mount_legs.aux.mix index 4bfa035d415c..c9a0ce49f0a4 100644 --- a/ROMFS/px4fmu_common/mixers/mount_legs.aux.mix +++ b/ROMFS/px4fmu_common/mixers/mount_legs.aux.mix @@ -1,3 +1,4 @@ +# @board px4_fmu-v2 exclude # Roll channel for mount M: 1 diff --git a/ROMFS/px4fmu_common/mixers/octo_cox_w.main.mix b/ROMFS/px4fmu_common/mixers/octo_cox_w.main.mix index 5c79dc615390..c6ef858590f5 100644 --- a/ROMFS/px4fmu_common/mixers/octo_cox_w.main.mix +++ b/ROMFS/px4fmu_common/mixers/octo_cox_w.main.mix @@ -1,3 +1,4 @@ # Octo coaxial with wide arms +# @board px4_fmu-v2 exclude R: 8cw diff --git a/ROMFS/px4fmu_common/mixers/phantom.main.mix b/ROMFS/px4fmu_common/mixers/phantom.main.mix deleted file mode 100644 index 0e5c2df55c0e..000000000000 --- a/ROMFS/px4fmu_common/mixers/phantom.main.mix +++ /dev/null @@ -1,45 +0,0 @@ -Phantom FX-61 mixer -=================== - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4/Pixhawk. The configuration assumes the elevon servos are connected to -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor are set so that pitch will have more travel than roll. - -M: 2 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 6500 6500 0 -10000 10000 - -M: 2 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 -6500 -6500 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix index 8705668f0a1f..ae8868892558 100644 --- a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -1,5 +1,7 @@ Mixer for Tailsitter with x motor configuration and elevons =========================================================== +# @board px4_fmu-v2 exclude +# @board omnibus_f4sd exclude This file defines a single mixer for tailsitter with motors in X configuration. All controls are mixed 100%. diff --git a/ROMFS/px4fmu_common/mixers/tilt_quad.aux.mix b/ROMFS/px4fmu_common/mixers/tilt_quad.aux.mix index e6f63c98e95b..fd4d3d9068e3 100644 --- a/ROMFS/px4fmu_common/mixers/tilt_quad.aux.mix +++ b/ROMFS/px4fmu_common/mixers/tilt_quad.aux.mix @@ -1,6 +1,8 @@ Tilt-Quadrotor mixer for PX4FMU (2/2) V2 =========================== +# @board px4_fmu-v2 exclude + This file defines the aux outputs mixer for a Tilt-quadrotor in the + configuration. # @output AUX1 Outer servo motor for rotor 2 arm diff --git a/ROMFS/px4fmu_common/mixers/tilt_quad.main.mix b/ROMFS/px4fmu_common/mixers/tilt_quad.main.mix index 9c7b9b73dd6f..5d7cf9acb13c 100644 --- a/ROMFS/px4fmu_common/mixers/tilt_quad.main.mix +++ b/ROMFS/px4fmu_common/mixers/tilt_quad.main.mix @@ -1,6 +1,8 @@ Tilt-Quadrotor mixer for PX4FMU (1/2) V2 =========================== +# @board px4_fmu-v2 exclude + This file defines the main outputs mixer for a Tilt-quadrotor in the + configuration. # @output MAIN1 motor 1 diff --git a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix index 0ef3b6501ce2..47d8228645df 100644 --- a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix +++ b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix @@ -1,3 +1,5 @@ +# @board px4_fmu-v2 exclude + # Motor 1 M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix index 84d99b925aeb..efef782992be 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_AAERT.aux.mix @@ -1,6 +1,8 @@ Mixer for an AAERT VTOL ======================= +# @board px4_fmu-v2 exclude + Aileron 1 mixer --------------- M: 1 diff --git a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix index 1991ebd452dc..e59584b21348 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix @@ -1,16 +1,18 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU ======================================================= +# @board px4_fmu-v2 exclude + This file defines mixers suitable for controlling a fixed wing aircraft with aileron, v-tail (rudder, elevator) and throttle using PX4FMU. -The configuration assumes the aileron servos are connected to PX4FMU +The configuration assumes the aileron servos are connected to PX4FMU AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -20,11 +22,11 @@ endpoints to suit. M: 2 S: 1 0 -10000 -10000 0 -10000 10000 -S: 1 6 10000 10000 0 -10000 10000 +S: 1 5 10000 10000 0 -10000 10000 M: 2 S: 1 0 -10000 -10000 0 -10000 10000 -S: 1 6 -10000 -10000 0 -10000 10000 +S: 1 5 -10000 -10000 0 -10000 10000 V-tail mixers diff --git a/ROMFS/px4fmu_common/mixers/vtol_TTTTAAER.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_TTTTAAER.aux.mix index 27403d016b30..fe684c70bc2f 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_TTTTAAER.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_TTTTAAER.aux.mix @@ -1,22 +1,24 @@ # Generic quadplane tiltrotor servo mixer +# @board px4_fmu-v2 exclude + # Tilt mechanism servo mixer --------------------------- # front left up:2000 down:1000 M: 1 -S: 1 4 0 -20000 10000 -10000 10000 +S: 1 8 0 -20000 10000 -10000 10000 # front right up:1000 down:2000 M: 1 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # rear left up:2000 down:1000 M: 1 -S: 1 4 0 -20000 10000 -10000 10000 +S: 1 8 0 -20000 10000 -10000 10000 # rear right up:1000 down:2000 M: 1 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 # Aileron mixer diff --git a/ROMFS/px4fmu_common/mixers/vtol_convergence.main.mix b/ROMFS/px4fmu_common/mixers/vtol_convergence.main.mix index 81c376a3732f..086150c257a6 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_convergence.main.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_convergence.main.mix @@ -1,4 +1,5 @@ # E-flite Convergence Tricopter Y-Configuration Mixer +# @board px4_fmu-v2 exclude # Motors R: 3y @@ -9,12 +10,12 @@ Tilt mechanism servo mixer --------------------------- #RIGHT up:2000 down:1000 M: 2 -S: 1 4 0 -20000 10000 -10000 10000 +S: 1 8 0 -20000 10000 -10000 10000 S: 0 2 8000 8000 0 -10000 10000 #LEFT up:1000 down:2000 M: 2 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 S: 0 2 8000 8000 0 -10000 10000 Elevon mixers diff --git a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix index 4ef04f2498ee..af248f7ce043 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_delta.aux.mix @@ -1,5 +1,6 @@ Delta-wing VTOL mixer ===================== +# @board px4_fmu-v2 exclude This file defines mixers suitable for controlling a delta wing VTOL aircraft using PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix index ab216c63c57c..0a5c6efd3734 100644 --- a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix +++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix @@ -1,5 +1,6 @@ Tailsitter duo mixer ============================ +# @board px4_fmu-v2 exclude This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle has two motors in total, one attached to each wing. It also has two elevons which diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix new file mode 100644 index 000000000000..2a9e3197c193 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix @@ -0,0 +1,39 @@ +Tailsitter duo mixer +============================ + +This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle +has two motors in total, one attached to each wing. It also has two elevons which +are located in the slipstream of the propellers. This mixer generates 4 PWM outputs +on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the +elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run +at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used. + +Motor mixer +------------ +Channel 1 connects to the right (starboard) motor. +Channel 2 connects to the left (port) motor. + +R: 2- + +Zero mixer (2x) +--------------- +Channels 3,4 are unused. + +Z: + +Z: + +Elevons mixer +-------------- +Channel 5 connects to the right (starboard) elevon. +Channel 6 connects to the left (port) elevon. +Here we saturate the elevons before their full range +to avoid roll-pitch-yaw coupling during faster maneuvers + +M: 2 +S: 1 0 10000 10000 0 -6000 6000 +S: 1 1 10000 10000 0 -6000 6000 + +M: 2 +S: 1 0 10000 10000 0 -6000 6000 +S: 1 1 -10000 -10000 0 -6000 6000 diff --git a/ROMFS/px4fmu_common/mixers/wingwing.main.mix b/ROMFS/px4fmu_common/mixers/wingwing.main.mix deleted file mode 100644 index 8b937599a230..000000000000 --- a/ROMFS/px4fmu_common/mixers/wingwing.main.mix +++ /dev/null @@ -1,48 +0,0 @@ -Delta-wing mixer for PX4FMU -=========================== - -Designed for Wing Wing Z-84 - -This file defines mixers suitable for controlling a delta wing aircraft using -PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU -servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is -assumed to be unused. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). - -See the README for more information on the scaler format. - -Elevon mixers -------------- -Three scalers total (output, roll, pitch). - -On the assumption that the two elevon servos are physically reversed, the pitch -input is inverted between the two servos. - -The scaling factor for roll inputs is adjusted to implement differential travel -for the elevons. - -M: 2 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 6500 6500 0 -10000 10000 - -M: 2 -S: 0 0 -6000 -6000 0 -10000 10000 -S: 0 1 -6500 -6500 0 -10000 10000 - -Output 2 --------- -This mixer is empty. - -Z: - -Motor speed mixer ------------------ -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. - -M: 1 -S: 0 3 0 20000 -10000 -10000 10000 diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index 4291deaf298a..ab856ee2a78a 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -16,15 +16,15 @@ if board_adc start then fi -if sdp3x_airspeed start -X +if sdp3x start -X then fi -if ms5525_airspeed start -X +if ms5525dso start -X then fi -if ms4525_airspeed start -X +if ms4525do start -X then fi diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index dfd74cd828f1..2a146a3e8365 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -13,14 +13,6 @@ then led_control on -c blue fi -if sercon -then - echo "[i] USB interface connected" - - # Try to get an USB console - nshterm /dev/ttyACM0 & -fi - # # Try to mount the microSD card. # @@ -70,14 +62,6 @@ then set io_file /etc/extras/px4_io-v2_default.bin fi -if px4io start -then - echo "PX4IO OK" -else - set unit_test_failure 1 - set unit_test_failure_list "${unit_test_failure_list} px4io_start" -fi - if px4io checkcrc $io_file then echo "PX4IO CRC OK" @@ -104,6 +88,15 @@ else fi fi +if px4io start +then + echo "PX4IO OK" +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} px4io_start" +fi + + # # The presence of this file suggests we're running a mount stress test # diff --git a/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix b/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix index 2d6cf285f0fa..150e95d8941e 100644 --- a/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix +++ b/ROMFS/px4fmu_test/mixers/AAERTWF.main.mix @@ -10,7 +10,7 @@ to output 4 and the wheel to output 5. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,12 +21,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 Elevator mixer ------------ diff --git a/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix b/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix index e6520862d727..531cccaf1a4a 100644 --- a/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix +++ b/ROMFS/px4fmu_test/mixers/AAVVTWFF.main.mix @@ -9,9 +9,9 @@ output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4, the wheel to output 5 and the flaps to output 6 and 7. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -22,12 +22,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 0 0 10000 10000 0 -10000 10000 -S: 0 6 -10000 -10000 0 -10000 10000 +S: 0 5 -10000 -10000 0 -10000 10000 V-tail mixers ------------- diff --git a/ROMFS/px4fmu_test/mixers/vtol2_test.mix b/ROMFS/px4fmu_test/mixers/vtol2_test.mix index d49070f5b987..f9ed50e6c999 100644 --- a/ROMFS/px4fmu_test/mixers/vtol2_test.mix +++ b/ROMFS/px4fmu_test/mixers/vtol2_test.mix @@ -10,13 +10,13 @@ Tilt mechanism servo mixer #RIGHT up:2000 down:1000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 4 0 -20000 9000 -10000 10000 +S: 1 8 0 -20000 9000 -10000 10000 S: 0 2 4000 4000 0 -10000 10000 #LEFT up:1000 down:2000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 S: 0 2 4000 4000 0 -10000 10000 Elevon mixers diff --git a/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix b/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix index 86581585c295..a9026ebd659a 100644 --- a/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix +++ b/ROMFS/px4fmu_test/mixers/vtol_AAVVT.aux.mix @@ -3,14 +3,14 @@ Aileron/v-tail/throttle VTOL mixer for PX4FMU This file defines mixers suitable for controlling a fixed wing aircraft with aileron, v-tail (rudder, elevator) and throttle using PX4FMU. -The configuration assumes the aileron servos are connected to PX4FMU +The configuration assumes the aileron servos are connected to PX4FMU AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle to output 4. Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon). +(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 5 (spoiler). -Aileron mixer (roll + flaperon) +Aileron mixer (roll + spoiler) --------------------------------- This mixer assumes that the aileron servos are set up correctly mechanically; @@ -21,12 +21,12 @@ endpoints to suit. M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 10000 10000 0 -10000 10000 -S: 1 6 10000 10000 0 -10000 10000 +S: 1 5 10000 10000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 10000 10000 0 -10000 10000 -S: 1 6 -10000 -10000 0 -10000 10000 +S: 1 5 -10000 -10000 0 -10000 10000 V-tail mixers diff --git a/ROMFS/px4fmu_test/mixers/vtol_convergence.main.mix b/ROMFS/px4fmu_test/mixers/vtol_convergence.main.mix index a2d8a90621e8..ed9b69360b7c 100644 --- a/ROMFS/px4fmu_test/mixers/vtol_convergence.main.mix +++ b/ROMFS/px4fmu_test/mixers/vtol_convergence.main.mix @@ -10,13 +10,13 @@ Tilt mechanism servo mixer #RIGHT up:2000 down:1000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 4 0 -20000 10000 -10000 10000 +S: 1 8 0 -20000 10000 -10000 10000 S: 0 2 8000 8000 0 -10000 10000 #LEFT up:1000 down:2000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 4 0 20000 -10000 -10000 10000 +S: 1 8 0 20000 -10000 -10000 10000 S: 0 2 8000 8000 0 -10000 10000 Elevon mixers diff --git a/Tools/HIL/monitor_firmware_upload.py b/Tools/HIL/monitor_firmware_upload.py index fcad71a41be6..6e05d05dbcec 100755 --- a/Tools/HIL/monitor_firmware_upload.py +++ b/Tools/HIL/monitor_firmware_upload.py @@ -5,53 +5,106 @@ from subprocess import call, Popen from argparse import ArgumentParser import re +import sys +import datetime +import serial.tools.list_ports as list_ports +import tempfile -def monitor_firmware_upload(port, baudrate): - databits = serial.EIGHTBITS - stopbits = serial.STOPBITS_ONE - parity = serial.PARITY_NONE - ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=1) +COLOR_RED = "\x1b[31m" +COLOR_GREEN = "\x1b[32m" +COLOR_YELLOW = "\x1b[33m" +COLOR_WHITE = "\x1b[37m" +COLOR_RESET = "\x1b[0m" - finished = 0 +def print_line(line): + if "WARNING" in line: + line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1) + elif "WARN" in line: + line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1) + elif "ERROR" in line: + line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1) + elif "INFO" in line: + line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1) - timeout = 300 # 5 minutes - timeout_start = time.time() - timeout_newline = time.time() + if "PASSED" in line: + line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1) - while finished == 0: + if "FAILED" in line: + line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1) + + if "\n" in line: + current_time = datetime.datetime.now() + print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='') + else: + print('{0}'.format(line), end='') + + +def monitor_firmware_upload(port_url, baudrate): + ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=3, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) + + timeout = 180 # 3 minutes + timeout_start = time.monotonic() + timeout_newline = time.monotonic() + + return_code = 0 + + while True: serial_line = ser.readline().decode("ascii", errors='ignore') - if (len(serial_line) > 0): - print(serial_line.replace('\n', '')) - if "NuttShell (NSH)" in serial_line: - finished = 1 - break + if len(serial_line) > 0: + + print_line(serial_line) - if time.time() - timeout_start > 10: - if "nsh>" in serial_line: - finished = 1 - break + if "NuttShell (NSH)" in serial_line: + sys.exit(return_code) + elif "nsh>" in serial_line: + sys.exit(return_code) - if time.time() > timeout_start + timeout: - print("Error, timeout") - finished = 1 - break + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout") + sys.exit(-1) - # newline every 10 seconds if still running - if time.time() - timeout_newline > 10: - timeout_newline = time.time() - ser.write('\n'.encode("ascii")) - ser.flush() + # newline every 10 seconds if still running + if (len(serial_line) <= 0) and (time.monotonic() - timeout_newline > 10): + timeout_newline = time.monotonic() + ser.write("\n".encode("ascii")) - ser.close() def main(): + + default_device = None + device_required = True + + # select USB UART as default if there's only 1 + ports = list(serial.tools.list_ports.grep('USB UART')) + + if (len(ports) == 1): + default_device = ports[0].device + device_required = False + + print("Default USB UART port: {0}".format(ports[0].name)) + print(" device: {0}".format(ports[0].device)) + print(" description: \"{0}\" ".format(ports[0].description)) + print(" hwid: {0}".format(ports[0].hwid)) + #print(" vid: {0}, pid: {1}".format(ports[0].vid, ports[0].pid)) + #print(" serial_number: {0}".format(ports[0].serial_number)) + #print(" location: {0}".format(ports[0].location)) + print(" manufacturer: {0}".format(ports[0].manufacturer)) + #print(" product: {0}".format(ports[0].product)) + #print(" interface: {0}".format(ports[0].interface)) + parser = ArgumentParser(description=__doc__) - parser.add_argument('--device', "-d", nargs='?', default=None, help='', required=True) - parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600) + parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) args = parser.parse_args() - monitor_firmware_upload(args.device, args.baudrate) + tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) + port_url = "spy://{0}?file={1}".format(args.device, tmp_file) + + print("pyserial url: {0}".format(port_url)) + + monitor_firmware_upload(port_url, args.baudrate) if __name__ == "__main__": main() diff --git a/Tools/HIL/nsh_param_set.py b/Tools/HIL/nsh_param_set.py new file mode 100755 index 000000000000..bba2e4072db5 --- /dev/null +++ b/Tools/HIL/nsh_param_set.py @@ -0,0 +1,171 @@ +#! /usr/bin/env python3 + +import serial, time +import subprocess +from subprocess import call, Popen +from argparse import ArgumentParser +import re +import sys +import datetime +import serial.tools.list_ports as list_ports +import tempfile + +COLOR_RED = "\x1b[31m" +COLOR_GREEN = "\x1b[32m" +COLOR_YELLOW = "\x1b[33m" +COLOR_WHITE = "\x1b[37m" +COLOR_RESET = "\x1b[0m" + +def print_line(line): + if "WARNING" in line: + line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1) + elif "WARN" in line: + line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1) + elif "ERROR" in line: + line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1) + elif "INFO" in line: + line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1) + + if "PASSED" in line: + line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1) + + if "FAILED" in line: + line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1) + + if "\n" in line: + current_time = datetime.datetime.now() + print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='') + else: + print('{0}'.format(line), end='') + + +def do_param_set_cmd(port_url, baudrate, param_name, param_value): + ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=3, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) + + timeout_start = time.monotonic() + timeout = 30 # 30 seconds + + ser.write("\n\n\n".encode("ascii")) + + # wait for nsh prompt + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + if "nsh>" in serial_line: + break + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for prompt") + sys.exit(1) + + ser.write("\n".encode("ascii")) + + + # clear + ser.reset_input_buffer() + + # run command + timeout_start = time.monotonic() + timeout = 10 # 10 seconds + + cmd = "param set " + param_name + " " + param_value + + # write command (param set) and wait for command echo + print("Running command: \'{0}\'".format(cmd)) + serial_cmd = '{0}\n'.format(cmd) + + ser.write(serial_cmd.encode("ascii")) + + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + if cmd in serial_line: + break + + print_line(serial_line) + + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for command echo") + break + + # clear + ser.reset_input_buffer() + + # verify param value + cmd = "param show " + param_name + print("Running command: \'{0}\'".format(cmd)) + serial_cmd = '{0}\n'.format(cmd) + ser.write(serial_cmd.encode("ascii")) + + param_show_response = param_name + " [" + + timeout_start = time.monotonic() + timeout = 3 # 3 seconds + + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + print_line(serial_line) + + if param_show_response in serial_line: + current_param_value = serial_line.split(":")[-1].strip() + + if (current_param_value == param_value): + sys.exit(0) + else: + sys.exit(1) + else: + if time.monotonic() > timeout_start + timeout: + if "nsh>" in serial_line: + sys.exit(1) # error, command didn't complete successfully + elif "NuttShell (NSH)" in serial_line: + sys.exit(1) # error, command didn't complete successfully + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout") + sys.exit(-1) + + +def main(): + + default_device = None + device_required = True + + # select USB UART as default if there's only 1 + ports = list(serial.tools.list_ports.grep('USB UART')) + + if (len(ports) == 1): + default_device = ports[0].device + device_required = False + + print("Default USB UART port: {0}".format(ports[0].name)) + print(" device: {0}".format(ports[0].device)) + print(" description: \"{0}\" ".format(ports[0].description)) + print(" hwid: {0}".format(ports[0].hwid)) + #print(" vid: {0}, pid: {1}".format(ports[0].vid, ports[0].pid)) + #print(" serial_number: {0}".format(ports[0].serial_number)) + #print(" location: {0}".format(ports[0].location)) + print(" manufacturer: {0}".format(ports[0].manufacturer)) + #print(" product: {0}".format(ports[0].product)) + #print(" interface: {0}".format(ports[0].interface)) + + parser = ArgumentParser(description=__doc__) + parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) + parser.add_argument("--name", "-p", dest="param_name", help="Parameter name") + parser.add_argument("--value", "-v", dest="param_value", help="Parameter value") + args = parser.parse_args() + + tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) + port_url = "spy://{0}?file={1}".format(args.device, tmp_file) + + print("pyserial url: {0}".format(port_url)) + + do_param_set_cmd(port_url, args.baudrate, args.param_name, args.param_value) + +if __name__ == "__main__": + main() diff --git a/Tools/HIL/reboot.py b/Tools/HIL/reboot.py new file mode 100755 index 000000000000..421f3a7cf453 --- /dev/null +++ b/Tools/HIL/reboot.py @@ -0,0 +1,111 @@ +#! /usr/bin/env python3 + +import serial, time +import subprocess +from subprocess import call, Popen +from argparse import ArgumentParser +import re +import sys +import datetime +import serial.tools.list_ports as list_ports +import tempfile + +COLOR_RED = "\x1b[31m" +COLOR_GREEN = "\x1b[32m" +COLOR_YELLOW = "\x1b[33m" +COLOR_WHITE = "\x1b[37m" +COLOR_RESET = "\x1b[0m" + +def print_line(line): + if "WARNING" in line: + line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1) + elif "WARN" in line: + line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1) + elif "ERROR" in line: + line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1) + elif "INFO" in line: + line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1) + + if "PASSED" in line: + line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1) + + if "FAILED" in line: + line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1) + + if "\n" in line: + current_time = datetime.datetime.now() + print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='') + else: + print('{0}'.format(line), end='') + + +def reboot(port_url, baudrate): + ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=3, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) + + time_start = time.monotonic() + + ser.write("\n\n\n".encode("ascii")) + + ser.write("reboot\n".encode("ascii")) + time_reboot_cmd = time_start + + timeout_reboot_cmd = 90 + timeout = 300 # 5 minutes + + return_code = 0 + + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + print_line(serial_line) + + if "ERROR" in serial_line: + return_code = -1 + + if "NuttShell (NSH)" in serial_line: + sys.exit(return_code) + + else: + if time.monotonic() > time_start + timeout: + print("Error, timeout") + sys.exit(-1) + + if time.monotonic() > time_reboot_cmd + timeout_reboot_cmd: + time_reboot_cmd = time.monotonic() + print("sending reboot cmd again") + ser.write("reboot\n".encode("ascii")) + + +def main(): + + default_device = None + device_required = True + + # select USB UART as default if there's only 1 + ports = list(serial.tools.list_ports.grep('USB UART')) + + if (len(ports) == 1): + default_device = ports[0].device + device_required = False + + print("Default USB UART port: {0}".format(ports[0].name)) + print(" device: {0}".format(ports[0].device)) + print(" description: \"{0}\" ".format(ports[0].description)) + print(" hwid: {0}".format(ports[0].hwid)) + #print(" vid: {0}, pid: {1}".format(ports[0].vid, ports[0].pid)) + #print(" serial_number: {0}".format(ports[0].serial_number)) + #print(" location: {0}".format(ports[0].location)) + print(" manufacturer: {0}".format(ports[0].manufacturer)) + #print(" product: {0}".format(ports[0].product)) + #print(" interface: {0}".format(ports[0].interface)) + + parser = ArgumentParser(description=__doc__) + parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600) + args = parser.parse_args() + + reboot(args.device, args.baudrate) + +if __name__ == "__main__": + main() diff --git a/Tools/HIL/run_nsh_cmd.py b/Tools/HIL/run_nsh_cmd.py index 5a92c03d157e..dcdd9ec1e7b3 100755 --- a/Tools/HIL/run_nsh_cmd.py +++ b/Tools/HIL/run_nsh_cmd.py @@ -6,87 +6,156 @@ from argparse import ArgumentParser import re import sys +import datetime +import serial.tools.list_ports as list_ports +import tempfile + +COLOR_RED = "\x1b[31m" +COLOR_GREEN = "\x1b[32m" +COLOR_YELLOW = "\x1b[33m" +COLOR_WHITE = "\x1b[37m" +COLOR_RESET = "\x1b[0m" + +def print_line(line): + if "WARNING" in line: + line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1) + elif "WARN" in line: + line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1) + elif "ERROR" in line: + line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1) + elif "INFO" in line: + line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1) + + if "PASSED" in line: + line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1) + + if "FAILED" in line: + line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1) + + if "\n" in line: + current_time = datetime.datetime.now() + print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='') + else: + print('{0}'.format(line), end='') + + +def do_nsh_cmd(port_url, baudrate, cmd): + ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) + + timeout_start = time.monotonic() + timeout = 30 # 30 seconds -def do_nsh_cmd(port, baudrate, cmd): - databits = serial.EIGHTBITS - stopbits = serial.STOPBITS_ONE - parity = serial.PARITY_NONE - ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=0.1) + ser.write("\n\n\n".encode("ascii")) + + # wait for nsh prompt + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + if "nsh>" in serial_line: + break + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for prompt") + sys.exit(1) + + ser.write("\n".encode("ascii")) - # run command - timeout_start = time.time() - timeout = 10 # 10 seconds # clear - ser.write("\n".encode("ascii")) - ser.flush() - ser.readline() + ser.reset_input_buffer() + + # run command + timeout_start = time.monotonic() + timeout = 5 # 5 seconds success_cmd = "cmd succeeded!" - serial_cmd = '{0}; echo "{1}"\n'.format(cmd, success_cmd) + # wait for command echo + print("Running command: \'{0}\'".format(cmd)) + serial_cmd = '{0}; echo "{1}"; echo "{2}";\n'.format(cmd, success_cmd, success_cmd) ser.write(serial_cmd.encode("ascii")) - ser.flush() - ser.readline() - - # TODO: require successful command echo - # while True: - # serial_cmd = '{0}; echo "{1}"\n'.format(cmd, success_cmd) - # ser.write(serial_cmd.encode("ascii")) - # ser.flush() - - # serial_line = ser.readline().decode("ascii", errors='ignore') - # if cmd in serial_line: - # break - # else: - # print(serial_line, end='') + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') - # if time.time() > timeout_start + timeout: - # print("Error, timeout") - # sys.exit(-1) - # break + if len(serial_line) > 0: + if cmd in serial_line: + break + elif serial_line.startswith(success_cmd) and len(serial_line) <= len(success_cmd) + 2: + print_line(serial_line) + # we missed the echo, but command ran and succeeded + sys.exit(0) + else: + print_line(serial_line) + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for command echo") + break - # time.sleep(1) + timeout_start = time.monotonic() + timeout = 600 # 10 minutes - timeout_start = time.time() - timeout = 30 # 30 seconds + return_code = 0 while True: serial_line = ser.readline().decode("ascii", errors='ignore') - if success_cmd in serial_line: - break + if len(serial_line) > 0: + if success_cmd in serial_line: + sys.exit(return_code) + else: + if "ERROR " in serial_line: + return_code = -1 + + print_line(serial_line) + + if "nsh>" in serial_line: + sys.exit(1) # error, command didn't complete successfully + elif "NuttShell (NSH)" in serial_line: + sys.exit(1) # error, command didn't complete successfully else: - if len(serial_line) > 0: - print(serial_line, end='') + if time.monotonic() > timeout_start + timeout: + print("Error, timeout") + sys.exit(-1) - if "nsh>" in serial_line: - #sys.exit(-1) # error, command didn't complete successfully - break # TODO: return error on failure - elif "NuttShell (NSH)" in serial_line: - #sys.exit(-1) # error, command didn't complete successfully - break # TODO: return error on failure - if len(serial_line) <= 0: - ser.write("\n".encode("ascii")) - ser.flush() +def main(): - if time.time() > timeout_start + timeout: - print("Error, timeout") - sys.exit(-1) + default_device = None + device_required = True - ser.close() + # select USB UART as default if there's only 1 + ports = list(serial.tools.list_ports.grep('USB UART')) + + if (len(ports) == 1): + default_device = ports[0].device + device_required = False + + print("Default USB UART port: {0}".format(ports[0].name)) + print(" device: {0}".format(ports[0].device)) + print(" description: \"{0}\" ".format(ports[0].description)) + print(" hwid: {0}".format(ports[0].hwid)) + #print(" vid: {0}, pid: {1}".format(ports[0].vid, ports[0].pid)) + #print(" serial_number: {0}".format(ports[0].serial_number)) + #print(" location: {0}".format(ports[0].location)) + print(" manufacturer: {0}".format(ports[0].manufacturer)) + #print(" product: {0}".format(ports[0].product)) + #print(" interface: {0}".format(ports[0].interface)) -def main(): parser = ArgumentParser(description=__doc__) - parser.add_argument('--device', "-d", nargs='?', default=None, help='', required=True) - parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600) + parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) parser.add_argument("--cmd", "-c", dest="cmd", help="Command to run") args = parser.parse_args() - do_nsh_cmd(args.device, args.baudrate, args.cmd) + tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) + port_url = "spy://{0}?file={1}".format(args.device, tmp_file) + + print("pyserial url: {0}".format(port_url)) + + do_nsh_cmd(port_url, args.baudrate, args.cmd) if __name__ == "__main__": main() diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3f1c6076fccb..3e79c7022726 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -8,84 +8,126 @@ import unittest import os import sys +import datetime +import serial.tools.list_ports as list_ports +import tempfile +import warnings -def do_test(port, baudrate, test_name): - databits = serial.EIGHTBITS - stopbits = serial.STOPBITS_ONE - parity = serial.PARITY_NONE - ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=1) +COLOR_RED = "\x1b[31m" +COLOR_GREEN = "\x1b[32m" +COLOR_YELLOW = "\x1b[33m" +COLOR_WHITE = "\x1b[37m" +COLOR_RESET = "\x1b[0m" - success = False +def print_line(line): + if "WARNING" in line: + line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1) + elif "WARN" in line: + line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1) + elif "ERROR" in line: + line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1) + elif "INFO" in line: + line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1) + + if "PASSED" in line: + line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1) + + if "FAILED" in line: + line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1) + + if "\n" in line: + current_time = datetime.datetime.now() + print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='') + else: + print('{0}'.format(line), end='') + + +def do_test(port_url, baudrate, test_name): + + # ignore pyserial spy:// resource warnings + warnings.filterwarnings(action="ignore", message="unclosed", category=ResourceWarning) + + ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=3, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) + + timeout_start = time.monotonic() + timeout = 30 # 30 seconds + + ser.write("\n\n\n".encode("ascii")) + + # wait for nsh prompt + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + + if len(serial_line) > 0: + if "nsh>" in serial_line: + break + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for prompt") + return False + + ser.write("\n".encode("ascii")) + + + # clear + ser.reset_input_buffer() # run test cmd print('\n|======================================================================') cmd = 'tests ' + test_name print("| Running:", cmd) print('|======================================================================') - timeout_start = time.time() - timeout = 10 # 10 seconds - # clear - ser.write("\n".encode("ascii")) - ser.flush() - ser.readline() + timeout_start = time.monotonic() + timeout = 2 # 2 seconds + # wait for command echo + print("Running command: \'{0}\'".format(cmd)) serial_cmd = '{0}\n'.format(cmd) ser.write(serial_cmd.encode("ascii")) - ser.flush() - ser.readline() - - # TODO: retry command - # while True: - # serial_cmd = '{0}\n'.format(cmd) - # ser.write(serial_cmd.encode("ascii")) - # ser.flush() - - # serial_line = ser.readline().decode("ascii", errors='ignore') - - # if cmd in serial_line: - # break - # else: - # print(serial_line.replace('\n', '')) - # if time.time() > timeout_start + timeout: - # print("Error, unable to write cmd") - # return False - - # time.sleep(1) + while True: + serial_line = ser.readline().decode("ascii", errors='ignore') + if len(serial_line) > 0: + if cmd in serial_line: + break + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout waiting for command echo") + break # print results, wait for final result (PASSED or FAILED) - timeout = 180 # 3 minutes - timeout_start = time.time() + timeout = 300 # 5 minutes + timeout_start = time.monotonic() timeout_newline = timeout_start while True: serial_line = ser.readline().decode("ascii", errors='ignore') - if (len(serial_line) > 0): - print(serial_line, end='') - - if test_name + " PASSED" in serial_line: - success = True - break - elif test_name + " FAILED" in serial_line: - success = False - break - - if time.time() > timeout_start + timeout: - print("Error, timeout") - print(test_name + " FAILED") - success = False - break - - # newline every 10 seconds if still running - if time.time() - timeout_newline > 10: - ser.write("\n".encode("ascii")) - timeout_newline = time.time() - ser.close() + if len(serial_line) > 0: + print_line(serial_line) + + if test_name + " PASSED" in serial_line: + ser.close() + return True + elif test_name + " FAILED" in serial_line: + ser.close() + return False + else: + if time.monotonic() > timeout_start + timeout: + print("Error, timeout") + print(test_name + f" {COLOR_RED}FAILED{COLOR_RESET}") + ser.close() + return False + + # newline every 30 seconds if still running + if time.monotonic() - timeout_newline > 30: + ser.write("\n".encode("ascii")) + timeout_newline = time.monotonic() - return success + ser.close() + return False class TestHardwareMethods(unittest.TestCase): TEST_DEVICE = 0 @@ -103,15 +145,27 @@ def test_bitset(self): def test_bson(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bson")) - # def test_dataman(self): - # self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "dataman")) + def test_dataman(self): + self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "dataman")) - def floattest_float(self): + # def test_file(self): + # self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "file")) + + def test_file2(self): + self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "file2")) + + def test_float(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "float")) def test_hrt(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "hrt")) + def test_int(self): + self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "int")) + + def test_i2c_spi_cli(self): + self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "i2c_spi_cli")) + def test_IntrusiveQueue(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "IntrusiveQueue")) @@ -127,21 +181,6 @@ def test_mathlib(self): def test_matrix(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "matrix")) - def test_microbench_atomic(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "microbench_atomic")) - - def test_microbench_hrt(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "microbench_hrt")) - - def test_microbench_math(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "microbench_math")) - - def test_microbench_matrix(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "microbench_matrix")) - - def test_microbench_uorb(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "microbench_uorb")) - # def test_mixer(self): # self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "mixer")) @@ -170,12 +209,39 @@ def test_versioning(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "versioning")) def main(): + + default_device = None + device_required = True + + # select USB UART as default if there's only 1 + ports = list(serial.tools.list_ports.grep('USB UART')) + + if (len(ports) == 1): + default_device = ports[0].device + device_required = False + + print("Default USB UART port: {0}".format(ports[0].name)) + print(" device: {0}".format(ports[0].device)) + print(" description: \"{0}\" ".format(ports[0].description)) + print(" hwid: {0}".format(ports[0].hwid)) + #print(" vid: {0}, pid: {1}".format(ports[0].vid, ports[0].pid)) + #print(" serial_number: {0}".format(ports[0].serial_number)) + #print(" location: {0}".format(ports[0].location)) + print(" manufacturer: {0}".format(ports[0].manufacturer)) + #print(" product: {0}".format(ports[0].product)) + #print(" interface: {0}".format(ports[0].interface)) + parser = ArgumentParser(description=__doc__) - parser.add_argument('--device', "-d", nargs='?', default=None, help='', required=True) - parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600) + parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) + parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) args = parser.parse_args() - TestHardwareMethods.TEST_DEVICE = args.device + tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) + port_url = "spy://{0}?file={1}".format(args.device, tmp_file) + + print("pyserial url: {0}".format(port_url)) + + TestHardwareMethods.TEST_DEVICE = port_url TestHardwareMethods.TEST_BAUDRATE = args.baudrate unittest.main(__name__, failfast=True, verbosity=0, argv=['main']) diff --git a/Tools/HIL/test_airframes.sh b/Tools/HIL/test_airframes.sh new file mode 100755 index 000000000000..2119d09034f0 --- /dev/null +++ b/Tools/HIL/test_airframes.sh @@ -0,0 +1,62 @@ +#! /bin/bash + +# exit when any command fails +set -e + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +SERIAL_DEVICE=$1 + +if [ ! -e "${SERIAL_DEVICE}" ] +then + echo "Invalid serial device ${SERIAL_DEVICE}" + exit -1 +fi + +# all airframes (from ROMFS/px4fmu_common/init.d/airframes/) +# $(find . -regex '.*/[0-9].*' -exec basename {} \; | cut -d "_" -f 1) +ALL_AIRFRAMES=${@:2} +echo "airframes: ${ALL_AIRFRAMES}" + +for airframe in $ALL_AIRFRAMES +do + echo + echo + echo + echo "########################################################################################################################" + echo " Airframe: $airframe" + echo "########################################################################################################################" + echo + + ${DIR}/nsh_param_set.py --device ${SERIAL_DEVICE} --name SYS_AUTOSTART --value $airframe + ${DIR}/nsh_param_set.py --device ${SERIAL_DEVICE} --name CBRK_BUZZER --value 782097 + + # enable all mavlink instances + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_0_CONFIG 101' || true + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_1_CONFIG 102' || true + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_2_CONFIG 103' || true + + # enable all GPS + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set GPS_1_CONFIG 201' || true + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set GPS_1_CONFIG 202' || true + + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param reset SYS_HITL' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param status' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param save' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump' + + ${DIR}/reboot.py --device ${SERIAL_DEVICE} + + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param status' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump /fs/mtd_params' || true + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param dump /fs/microsd/parameters_backup.bson' || true + + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'ps' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'work_queue status' + + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'pwm info' + + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'mavlink stop-all' + ${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'gps stop' + +done diff --git a/Tools/astyle/check_code_style.sh b/Tools/astyle/check_code_style.sh index 0e96f754d7ab..3a2fcef35be8 100755 --- a/Tools/astyle/check_code_style.sh +++ b/Tools/astyle/check_code_style.sh @@ -8,18 +8,16 @@ if [ -f "$FILE" ]; then if [ -n "$CHECK_FAILED" ]; then ${DIR}/fix_code_style.sh --quiet < $FILE > $FILE.pretty + echo -e 'Formatting issue found in' $FILE echo - git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty + git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty | grep -vE -e "^.{,4}diff.*\.pretty.{,3}$" -e "^.{,4}--- a/.*$" -e "^.{,4}\+\+\+ b/.*$" -e "^.{,5}@@ .* @@.*$" -e "^.{,4}index .{10}\.\." rm -f $FILE.pretty echo if [[ $PX4_ASTYLE_FIX -eq 1 ]]; then ${DIR}/fix_code_style.sh $FILE else - # Make sure this file is not staged for comitting - git reset $FILE - # Provide instructions - echo $FILE 'bad formatting, please run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"' + echo 'to fix automatically run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"' exit 1 fi fi diff --git a/Tools/astyle/check_code_style_all.sh b/Tools/astyle/check_code_style_all.sh index 79c9d98bf7bb..5ace9ea52b10 100755 --- a/Tools/astyle/check_code_style_all.sh +++ b/Tools/astyle/check_code_style_all.sh @@ -59,7 +59,7 @@ if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then fi fi -${DIR}/files_to_check_code_style.sh | xargs -n 1 -P 8 -I % ${DIR}/check_code_style.sh % +${DIR}/files_to_check_code_style.sh | xargs -P 8 -I % ${DIR}/check_code_style.sh % if [ $? -eq 0 ]; then echo "Format checks passed" diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index ba3557aea0d2..6f77a514abd5 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -12,14 +12,19 @@ exec find boards msg src platforms test \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ - -path src/drivers/uavcan_v1/libcanard -prune -o \ - -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ - -path src/lib/ecl -prune -o \ - -path src/lib/matrix -prune -o \ + -path src/drivers/cyphal/libcanard -prune -o \ + -path src/lib/crypto/monocypher -prune -o \ + -path src/lib/events/libevents -prune -o \ -path src/lib/parameters/uthash -prune -o \ + -path src/modules/ekf2/EKF -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ + -path src/modules/mavlink/mavlink -prune -o \ -path src/modules/micrortps_bridge/micro-CDR -prune -o \ -path src/modules/micrortps_bridge/microRTPS_client -prune -o \ -path test/mavsdk_tests/catch2 -prune -o \ + -path src/lib/crypto/monocypher -prune -o \ + -path src/lib/crypto/libtomcrypt -prune -o \ + -path src/lib/crypto/libtommath -prune -o \ + -path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \ -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN diff --git a/Tools/astyle/pre-commit b/Tools/astyle/pre-commit index a07c820d269a..6c9bb248e62a 100755 --- a/Tools/astyle/pre-commit +++ b/Tools/astyle/pre-commit @@ -54,12 +54,9 @@ if [ $? -ne 0 ]; then fi # Check for code style, only in changed files -for i in `git diff --cached --name-only --diff-filter=ACM` -do - ./Tools/astyle/files_to_check_code_style.sh $i | xargs -n 1 -P 8 -I % ./Tools/astyle/check_code_style.sh % - if [ $? -ne 0 ] - then - echo "Pre-commit style error: Bad formatting according to astyle rules" - exit 1 - fi -done +bash -c "comm -12 <(./Tools/astyle/files_to_check_code_style.sh | sort) <(git diff --cached --name-only --diff-filter=ACM) | xargs -P 8 -I % ./Tools/astyle/check_code_style.sh %" +if [ $? -ne 0 ] +then + echo "Pre-commit style error: Bad formatting according to astyle rules" + exit 1 +fi diff --git a/Tools/bloaty_static_ram.bloaty b/Tools/bloaty_static_ram.bloaty new file mode 100644 index 000000000000..5d6908075e05 --- /dev/null +++ b/Tools/bloaty_static_ram.bloaty @@ -0,0 +1,13 @@ +custom_data_source: { + name: "bloaty_static_ram" + base_data_source: "sections" + + rewrite: { + pattern: "^\\.bss" + replacement: "ram" + } + rewrite: { + pattern: "^\\.data" + replacement: "ram" + } +} diff --git a/Tools/build_micrortps_agent.sh b/Tools/build_micrortps_agent.sh new file mode 100755 index 000000000000..6d45eaeb412e --- /dev/null +++ b/Tools/build_micrortps_agent.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +set -e + +SCRIPT_DIR=$0 +if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then + SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0")) +fi + +PX4_DIR=$(cd "$(dirname $(dirname $SCRIPT_DIR))" && pwd) + +if [ -d $PX4_DIR/build/*_rtps ]; then + cd $PX4_DIR/build/*_rtps/src/modules/micrortps_bridge/micrortps_agent/ + cmake -Bbuild + cmake --build build -j$(nproc --all) +fi diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 4304070e588e..8082491ee204 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -9,6 +9,7 @@ if [[ -f $1"/.git" || -d $1"/.git" ]]; then if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then git submodule --quiet sync --recursive -- $1 git submodule --quiet update --init --recursive --jobs=8 -- $1 || true + git submodule --quiet sync --recursive -- $1 git submodule --quiet update --init --recursive --jobs=8 -- $1 exit 0 fi @@ -51,6 +52,7 @@ if [[ -f $1"/.git" || -d $1"/.git" ]]; then else git submodule --quiet sync --recursive --quiet -- $1 git submodule --quiet update --init --recursive -- $1 || true + git submodule --quiet sync --recursive --quiet -- $1 git submodule --quiet update --init --recursive -- $1 fi diff --git a/Tools/compress.py b/Tools/compress.py new file mode 100755 index 000000000000..e4511e293fe7 --- /dev/null +++ b/Tools/compress.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python + +import argparse +import lzma + +parser = argparse.ArgumentParser(description="""Compress a file with xz""") +parser.add_argument('filename', metavar='file', help='Input file (output: file.xz)') + +args = parser.parse_args() +filename = args.filename + +def save_compressed(filename): + #create xz compressed version + xz_filename=filename+'.xz' + with lzma.open(xz_filename, 'wt', preset=9) as f: + with open(filename, 'r') as content_file: + f.write(content_file.read()) + +save_compressed(filename) diff --git a/Tools/cryptotools.py b/Tools/cryptotools.py index be6a90cec0af..6e4bd42d63e5 100755 --- a/Tools/cryptotools.py +++ b/Tools/cryptotools.py @@ -81,7 +81,8 @@ def sign(bin_file_path, key_file_path=None, generated_key_file=None): # Align to 4 bytes. Signature always starts at # 4 byte aligned address, but the signee size # might not be aligned - signee_bin += bytearray(b'\xff')*(4-len(signee_bin)%4) + if len(signee_bin)%4 != 0: + signee_bin += bytearray(b'\xff')*(4-len(signee_bin)%4) try: with open(key_file_path,mode='r') as f: @@ -133,7 +134,7 @@ def generate_key(key_file): parser.add_argument("signee", help=".bin file to add signature", nargs='?', default=None) parser.add_argument("signed", help="signed output .bin", nargs='?', default=None) - parser.add_argument("--key", help="key.json file", default="Tools/test_keys.json") + parser.add_argument("--key", help="key.json file", default="Tools/test_keys/test_keys.json") parser.add_argument("--rdct", help="binary R&D certificate file", default=None) parser.add_argument("--genkey", help="new generated key", default=None) args = parser.parse_args() @@ -152,7 +153,7 @@ def generate_key(key_file): sys.exit(1) # Issue a warning when signing with testing key - if args.key=='Tools/test_keys.json': + if args.key=='Tools/test_keys/test_keys.json': print("WARNING: Signing with PX4 test key") # Sign the binary diff --git a/Tools/decrypt_ulog.py b/Tools/decrypt_ulog.py new file mode 100755 index 000000000000..f6d888c56b62 --- /dev/null +++ b/Tools/decrypt_ulog.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +from Crypto.PublicKey import RSA +from Crypto.Cipher import PKCS1_OAEP +from Crypto.Cipher import ChaCha20 +from Crypto.Hash import SHA256 +import binascii +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("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); + + # 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: + ulog_key_header = f.read(22) + + # Parse the header + try: + # magic + if not ulog_key_header.startswith(bytearray("ULogKey".encode())): + raise Exception() + # version + if ulog_key_header[7] != 1: + raise Exception() + # expected key exchange algorithm (RSA_OAEP) + if ulog_key_header[16] != 4: + raise Exception() + 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); + + # 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 + cipher = ChaCha20.new(key=ulog_key, nonce=nonce) + with open(args.ulog_file, 'rb') as f: + with open(args.ulog_file.rstrip(args.ulog_file[-1]), 'wb') as out: + out.write(cipher.decrypt(f.read())) diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 949df471eba2..3ac640428ea4 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -4,25 +4,22 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then echo "guessing PX4_DOCKER_REPO based on input"; if [[ $@ =~ .*px4_fmu.* ]]; then # nuttx-px4fmu-v{1,2,3,4,5} - PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-02-04" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then # beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18" elif [[ $@ =~ .*pilotpi.arm64 ]]; then # scumaker_pilotpi_arm64 PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest" - elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then - # eagle, excelsior - PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2020-04-01" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then # posix_rpi_cross, posix_bebop_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18" elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then # clang tools PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04" elif [[ $@ =~ .*tests* ]]; then # run all tests with simulation - PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-02-04" + PX4_DOCKER_REPO="px4io/px4-dev-simulation-bionic:2021-12-11" fi else echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'"; @@ -30,7 +27,7 @@ fi # otherwise default to nuttx if [ -z ${PX4_DOCKER_REPO+x} ]; then - PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2020-09-14" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08" fi # docker hygiene diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index b33fd861043b..f0c33cfc8e41 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -11,7 +11,7 @@ from analysis.detectors import InAirDetector, PreconditionError from analysis.metrics import calculate_ecl_ekf_metrics from analysis.checks import perform_ecl_ekf_checks -from analysis.post_processing import get_estimator_check_flags +from analysis.post_processing import get_gps_check_fail_flags def analyse_ekf( ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0, @@ -40,6 +40,11 @@ def analyse_ekf( except: raise PreconditionError('could not find estimator_status instance', multi_instance) + try: + estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data + except: + raise PreconditionError('could not find estimator_status_flags instance', multi_instance) + try: _ = ulog.get_dataset('estimator_innovations', multi_instance).data except: @@ -61,14 +66,14 @@ def analyse_ekf( 'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2), 'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)} - control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) + gps_fail_flags = get_gps_check_fail_flags(estimator_status) sensor_checks, innov_fail_checks = find_checks_that_apply( - control_mode, estimator_status, + estimator_status_flags, estimator_status, pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused) metrics = calculate_ecl_ekf_metrics( - ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects, + ulog, estimator_status_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects, multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh) check_status, master_status = perform_ecl_ekf_checks( @@ -78,12 +83,12 @@ def analyse_ekf( def find_checks_that_apply( - control_mode: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\ + estimator_status_flags: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\ Tuple[List[str], List[str]]: """ finds the checks that apply and stores them in lists for the std checks and the innovation fail checks. - :param control_mode: + :param estimator_status_flags: :param estimator_status: :param b_pos_only_when_sensors_fused: :return: a tuple of two lists that contain strings for the std checks and for the innovation @@ -97,7 +102,7 @@ def find_checks_that_apply( innov_fail_checks.append('posv') # Magnetometer Sensor Checks - if (np.amax(control_mode['yaw_aligned']) > 0.5): + if (np.amax(estimator_status_flags['cs_yaw_align']) > 0.5): sensor_checks.append('mag') innov_fail_checks.append('magx') @@ -106,13 +111,14 @@ def find_checks_that_apply( innov_fail_checks.append('yaw') # Velocity Sensor Checks - if (np.amax(control_mode['using_gps']) > 0.5): + if (np.amax(estimator_status_flags['cs_gps']) > 0.5): sensor_checks.append('vel') - innov_fail_checks.append('vel') + innov_fail_checks.append('velh') + innov_fail_checks.append('velv') # Position Sensor Checks - if (pos_checks_when_sensors_not_fused or (np.amax(control_mode['using_gps']) > 0.5) - or (np.amax(control_mode['using_evpos']) > 0.5)): + if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5) + or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)): sensor_checks.append('pos') innov_fail_checks.append('posh') @@ -128,7 +134,7 @@ def find_checks_that_apply( innov_fail_checks.append('hagl') # optical flow sensor checks - if (np.amax(control_mode['using_optflow']) > 0.5): + if (np.amax(estimator_status_flags['cs_opt_flow']) > 0.5): innov_fail_checks.append('ofx') innov_fail_checks.append('ofy') diff --git a/Tools/ecl_ekf/analysis/checks.py b/Tools/ecl_ekf/analysis/checks.py index 9a1b20b96e11..ac0248a4c1c1 100644 --- a/Tools/ecl_ekf/analysis/checks.py +++ b/Tools/ecl_ekf/analysis/checks.py @@ -55,7 +55,7 @@ def perform_imu_checks( # perform the vibration check imu_status['imu_vibration_check'] = 'Pass' - for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']: + for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']: mean_metric = '{:s}_mean'.format(imu_vibr_metric) peak_metric = '{:s}_peak'.format(imu_vibr_metric) if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \ @@ -123,7 +123,8 @@ def perform_sensor_innov_checks( ('magy', 'magy_fail_percentage', 'mag'), ('magz', 'magz_fail_percentage', 'mag'), ('yaw', 'yaw_fail_percentage', 'yaw'), - ('vel', 'vel_fail_percentage', 'vel'), + ('velh', 'vel_fail_percentage', 'vel'), + ('velv', 'vel_fail_percentage', 'vel'), ('posh', 'pos_fail_percentage', 'pos'), ('tas', 'tas_fail_percentage', 'tas'), ('hagl', 'hagl_fail_percentage', 'hagl'), diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 22b6a619314d..6c07c12caa01 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -11,7 +11,7 @@ from analysis.detectors import InAirDetector def calculate_ecl_ekf_metrics( - ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str], + ulog: ULog, estimator_status_flags: Dict[str, float], innov_fail_checks: List[str], sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: @@ -20,7 +20,7 @@ def calculate_ecl_ekf_metrics( red_thresh=red_thresh, amb_thresh=amb_thresh) innov_fail_metrics = calculate_innov_fail_metrics( - innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects) + estimator_status_flags, innov_fail_checks, in_air, in_air_no_ground_effects) imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects) @@ -90,10 +90,10 @@ def calculate_sensor_metrics( def calculate_innov_fail_metrics( - innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector, + estimator_status_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector) -> dict: """ - :param innov_flags: + :param estimator_status_flags: :param innov_fail_checks: :param in_air: :param in_air_no_ground_effects: @@ -103,17 +103,18 @@ def calculate_innov_fail_metrics( innov_fail_metrics = dict() # calculate innovation check fail metrics - for signal_id, signal, result in [('posv', 'posv_innov_fail', 'hgt_fail_percentage'), - ('magx', 'magx_innov_fail', 'magx_fail_percentage'), - ('magy', 'magy_innov_fail', 'magy_fail_percentage'), - ('magz', 'magz_innov_fail', 'magz_fail_percentage'), - ('yaw', 'yaw_innov_fail', 'yaw_fail_percentage'), - ('vel', 'vel_innov_fail', 'vel_fail_percentage'), - ('posh', 'posh_innov_fail', 'pos_fail_percentage'), - ('tas', 'tas_innov_fail', 'tas_fail_percentage'), - ('hagl', 'hagl_innov_fail', 'hagl_fail_percentage'), - ('ofx', 'ofx_innov_fail', 'ofx_fail_percentage'), - ('ofy', 'ofy_innov_fail', 'ofy_fail_percentage')]: + for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'), + ('magx', 'reject_mag_x', 'magx_fail_percentage'), + ('magy', 'reject_mag_y', 'magy_fail_percentage'), + ('magz', 'reject_mag_z', 'magz_fail_percentage'), + ('yaw', 'reject_yaw', 'yaw_fail_percentage'), + ('velh', 'reject_hor_vel', 'vel_fail_percentage'), + ('velv', 'reject_ver_vel', 'vel_fail_percentage'), + ('posh', 'reject_hor_pos', 'pos_fail_percentage'), + ('tas', 'reject_airspeed', 'tas_fail_percentage'), + ('hagl', 'reject_hagl', 'hagl_fail_percentage'), + ('ofx', 'reject_optflow_x', 'ofx_fail_percentage'), + ('ofy', 'reject_optflow_y', 'ofy_fail_percentage')]: # only run innov fail checks, if they apply. if signal_id in innov_fail_checks: @@ -125,7 +126,7 @@ def calculate_innov_fail_metrics( in_air_detector = in_air innov_fail_metrics[result] = calculate_stat_from_signal( - innov_flags, 'estimator_status', signal, in_air_detector, + estimator_status_flags, 'estimator_status_flags', signal, in_air_detector, lambda x: 100.0 * np.mean(x > 0.5)) return innov_fail_metrics @@ -144,17 +145,27 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: imu_metrics[result] = calculate_stat_from_signal( estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median) + # calculates peak and mean for IMU vibration checks - for signal, result in [('vibe[0]', 'imu_coning'), - ('vibe[1]', 'imu_hfdang'), - ('vibe[2]', 'imu_hfdvel')]: - peak = calculate_stat_from_signal( - estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax) - if peak > 0.0: - imu_metrics['{:s}_peak'.format(result)] = peak - imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal( - estimator_status_data, 'estimator_status', signal, - in_air_no_ground_effects, np.mean) + for imu_status_instance in range(4): + try: + vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data + + if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]: + + for signal, result in [('gyro_coning_vibration', 'imu_coning'), + ('gyro_vibration_metric', 'imu_hfgyro'), + ('accel_vibration_metric', 'imu_hfaccel')]: + + peak = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.amax) + + if peak > 0.0: + imu_metrics['{:s}_peak'.format(result)] = peak + imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.mean) + + except: + pass + # IMU bias checks estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data diff --git a/Tools/ecl_ekf/analysis/post_processing.py b/Tools/ecl_ekf/analysis/post_processing.py index 2bb5da5cf860..26c0bbb34503 100644 --- a/Tools/ecl_ekf/analysis/post_processing.py +++ b/Tools/ecl_ekf/analysis/post_processing.py @@ -7,115 +7,6 @@ import numpy as np - -def get_estimator_check_flags(estimator_status: dict) -> Tuple[dict, dict, dict]: - """ - :param estimator_status: - :return: - """ - control_mode = get_control_mode_flags(estimator_status) - innov_flags = get_innovation_check_flags(estimator_status) - gps_fail_flags = get_gps_check_fail_flags(estimator_status) - return control_mode, innov_flags, gps_fail_flags - - -def get_control_mode_flags(estimator_status: dict) -> dict: - """ - :param estimator_status: - :return: - """ - - control_mode = dict() - # extract control mode metadata from estimator_status.control_mode_flags - # 0 - true if the filter tilt alignment is complete - # 1 - true if the filter yaw alignment is complete - # 2 - true if GPS measurements are being fused - # 3 - true if optical flow measurements are being fused - # 4 - true if a simple magnetic yaw heading is being fused - # 5 - true if 3-axis magnetometer measurement are being fused - # 6 - true if synthetic magnetic declination measurements are being fused - # 7 - true when the vehicle is airborne - # 8 - true when wind velocity is being estimated - # 9 - true when baro height is being fused as a primary height reference - # 10 - true when range finder height is being fused as a primary height reference - # 11 - true when range finder height is being fused as a primary height reference - # 12 - true when local position data from external vision is being fused - # 13 - true when yaw data from external vision measurements is being fused - # 14 - true when height data from external vision measurements is being fused - # 15 - true when synthetic sideslip measurements are being fused - # 16 - true true when the mag field does not match the expected strength - # 17 - true true when the vehicle is operating as a fixed wing vehicle - # 18 - true when the magnetometer has been declared faulty and is no longer being used - # 19 - true true when airspeed measurements are being fused - # 20 - true true when protection from ground effect induced static pressure rise is active - # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough - # 22 - true when yaw (not ground course) data from a GPS receiver is being fused - # 23 - true when the in-flight mag field alignment has been completed - # 24 - true when local earth frame velocity data from external vision measurements are being fused - # 25 - true when we are using a synthesized measurement for the magnetometer Z component - control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_optflow'] = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_magyaw'] = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_mag3d'] = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_magdecl'] = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['airborne'] = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['estimating_wind'] = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_barohgt'] = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_rnghgt'] = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_gpshgt'] = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1 - control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1 - return control_mode - -def get_innovation_check_flags(estimator_status: dict) -> dict: - """ - :param estimator_status: - :return: - """ - - innov_flags = dict() - # innovation_check_flags summary - # 0 - true if velocity observations have been rejected - # 1 - true if horizontal position observations have been rejected - # 2 - true if true if vertical position observations have been rejected - # 3 - true if the X magnetometer observation has been rejected - # 4 - true if the Y magnetometer observation has been rejected - # 5 - true if the Z magnetometer observation has been rejected - # 6 - true if the yaw observation has been rejected - # 7 - true if the airspeed observation has been rejected - # 8 - true if synthetic sideslip observation has been rejected - # 9 - true if the height above ground observation has been rejected - # 10 - true if the X optical flow observation has been rejected - # 11 - true if the Y optical flow observation has been rejected - innov_flags['vel_innov_fail'] = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['posh_innov_fail'] = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['posv_innov_fail'] = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['magx_innov_fail'] = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['magy_innov_fail'] = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['magz_innov_fail'] = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['yaw_innov_fail'] = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['tas_innov_fail'] = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['sli_innov_fail'] = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['hagl_innov_fail'] = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['ofx_innov_fail'] = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1 - innov_flags['ofy_innov_fail'] = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1 - return innov_flags - - def get_gps_check_fail_flags(estimator_status: dict) -> dict: """ :param estimator_status: diff --git a/Tools/ecl_ekf/batch_process_metadata_ekf.py b/Tools/ecl_ekf/batch_process_metadata_ekf.py index f6a5464dc0f4..2797c53401dd 100755 --- a/Tools/ecl_ekf/batch_process_metadata_ekf.py +++ b/Tools/ecl_ekf/batch_process_metadata_ekf.py @@ -48,7 +48,7 @@ def is_valid_directory(parser, arg): # # print out the check levels # print('\n'+'The following metadata loaded from '+filename+' were used'+'\n') -# val = population_data.get(filename, {}).get('imu_hfdang_mean') +# val = population_data.get(filename, {}).get('imu_hfgyro_mean') # print(val) # Open pdf file for plotting @@ -90,10 +90,10 @@ def is_valid_directory(parser, arg): 'ofy_fail_pct_avg':[float('NaN'),'The mean percentage of innovation test fails for the Y axis optical flow sensor'], 'imu_coning_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU delta angle coning vibration level (mrad)'], 'imu_coning_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta angle coning vibration level (mrad)'], -'imu_hfdang_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta angle vibration level (mrad)'], -'imu_hfdang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta angle vibration level (mrad)'], -'imu_hfdvel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta velocity vibration level (m/s)'], -'imu_hfdvel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta velocity vibration level (m/s)'], +'imu_hfgyro_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency gyro vibration level (rad/s)'], +'imu_hfgyro_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency gyro vibration level (rad/s)'], +'imu_hfaccel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency accel vibration level (m/s/s)'], +'imu_hfaccel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency accel vibration level (m/s/s)'], 'obs_ang_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer angular tracking error magnitude (mrad)'], 'obs_vel_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer velocity tracking error magnitude (m/s)'], 'obs_pos_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer position tracking error magnitude (m)'], @@ -360,54 +360,54 @@ def is_valid_directory(parser, arg): plt.close(8) # IMU high frequency delta angle vibration levels -temp = np.asarray([population_data[k].get('imu_hfdang_peak') for k in found_keys]) +temp = np.asarray([population_data[k].get('imu_hfgyro_peak') for k in found_keys]) result1 = 1000.0 * temp[np.isfinite(temp)] -temp = np.asarray([population_data[k].get('imu_hfdang_mean') for k in found_keys]) +temp = np.asarray([population_data[k].get('imu_hfgyro_mean') for k in found_keys]) result2 = 1000.0 * temp[np.isfinite(temp)] if (len(result1) > 0 and len(result2) > 0): - population_results['imu_hfdang_max_avg'][0] = np.mean(result1) - population_results['imu_hfdang_mean_avg'][0] = np.mean(result2) + population_results['imu_hfgyro_max_avg'][0] = np.mean(result1) + population_results['imu_hfgyro_mean_avg'][0] = np.mean(result2) plt.figure(9,figsize=(20,13)) plt.subplot(2,1,1) plt.hist(result1) - plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Peak") - plt.xlabel("imu_hfdang_max (mrad)") + plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Peak") + plt.xlabel("imu_hfgyro_max (rad/s)") plt.ylabel("Frequency") plt.subplot(2,1,2) plt.hist(result2) - plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Mean") - plt.xlabel("imu_hfdang_mean (mrad)") + plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Mean") + plt.xlabel("imu_hfgyro_mean (rad/s)") plt.ylabel("Frequency") pp.savefig() plt.close(9) -# IMU high frequency delta velocity vibration levels -temp = np.asarray([population_data[k].get('imu_hfdvel_peak') for k in found_keys]) +# IMU high frequency accel vibration levels +temp = np.asarray([population_data[k].get('imu_hfaccel_peak') for k in found_keys]) result1 = temp[np.isfinite(temp)] -temp = np.asarray([population_data[k].get('imu_hfdvel_mean') for k in found_keys]) +temp = np.asarray([population_data[k].get('imu_hfaccel_mean') for k in found_keys]) result2 = temp[np.isfinite(temp)] if (len(result1) > 0 and len(result2) > 0): - population_results['imu_hfdvel_max_avg'][0] = np.mean(result1) - population_results['imu_hfdvel_mean_avg'][0] = np.mean(result2) + population_results['imu_hfaccel_max_avg'][0] = np.mean(result1) + population_results['imu_hfaccel_mean_avg'][0] = np.mean(result2) plt.figure(10,figsize=(20,13)) plt.subplot(2,1,1) plt.hist(result1) - plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Peak") - plt.xlabel("imu_hfdvel_max (m/s)") + plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Peak") + plt.xlabel("imu_hfaccel_max (m/s/s)") plt.ylabel("Frequency") plt.subplot(2,1,2) plt.hist(result2) - plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Mean") - plt.xlabel("imu_hfdvel_mean (m/s)") + plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Mean") + plt.xlabel("imu_hfaccel_mean (m/s/s)") plt.ylabel("Frequency") pp.savefig() @@ -535,12 +535,12 @@ def is_valid_directory(parser, arg): 'hgt_sensor_status':['Pass','Height sensor check summary. This sensor data can be sourced from either Baro, GPS, range fidner or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], 'hgt_test_max':[float('NaN'),'The maximum in-flight value of the height sensor innovation consistency test ratio.'], 'hgt_test_mean':[float('NaN'),'The mean in-flight value of the height sensor innovation consistency test ratio.'], -'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad)'], -'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad)'], -'imu_hfdang_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'], -'imu_hfdang_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)'], -'imu_hfdvel_mean':[float('NaN'),'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], -'imu_hfdvel_peak':[float('NaN'),'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], +'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)'], +'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)'], +'imu_hfgyro_mean':[float('NaN'),'Mean in-flight value of the IMU gyro high frequency vibration metric (rad/s)'], +'imu_hfgyro_peak':[float('NaN'),'Peak in-flight value of the IMU gyro high frequency vibration metric (rad/s)'], +'imu_hfaccel_mean':[float('NaN'),'Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)'], +'imu_hfaccel_peak':[float('NaN'),'Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)'], 'imu_sensor_status':['Pass','IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], 'in_air_transition_time':[float('NaN'),'The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected.'], 'mag_percentage_amber':[float('NaN'),'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5.'], diff --git a/Tools/ecl_ekf/check_level_dict.csv b/Tools/ecl_ekf/check_level_dict.csv index e1ac34451db8..97976145e8c0 100644 --- a/Tools/ecl_ekf/check_level_dict.csv +++ b/Tools/ecl_ekf/check_level_dict.csv @@ -21,10 +21,10 @@ hagl_amber_warn_pct,5.0 tas_amber_warn_pct,5.0 imu_coning_peak_warn,1.8E-5 imu_coning_mean_warn,3.6E-6 -imu_hfdang_peak_warn,3.0E-3 -imu_hfdang_mean_warn,6.0E-4 -imu_hfdvel_peak_warn,9.0E-2 -imu_hfdvel_mean_warn,1.8E-2 +imu_hfgyro_peak_warn,12 +imu_hfgyro_mean_warn,2.4 +imu_hfaccel_peak_warn,360 +imu_hfaccel_mean_warn,72 obs_ang_err_median_warn,8.0E-3 obs_vel_err_median_warn,0.05 obs_pos_err_median_warn,0.15 diff --git a/Tools/ecl_ekf/check_table.csv b/Tools/ecl_ekf/check_table.csv index b111e233f04c..1c02836e6983 100644 --- a/Tools/ecl_ekf/check_table.csv +++ b/Tools/ecl_ekf/check_table.csv @@ -49,12 +49,12 @@ hagl_test_mean, The mean in-flight value of the height above ground sensor innov ofx_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test. ofy_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test. filter_faults_max, Largest recorded value of the filter internal fault bitmask. Should always be zero. -imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad) -imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad) -imu_hfdang_peak, Peak in-flight value of the IMU delta angle high frequency vibration metric (rad) -imu_hfdang_mean, Mean in-flight value of the IMU delta angle high frequency vibration metric (rad) -imu_hfdvel_peak, Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s) -imu_hfdvel_mean, Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s) +imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad^2) +imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad^2) +imu_hfgyro_peak, Peak in-flight value of the IMU accel high frequency vibration metric (rad/s) +imu_hfgyro_mean, Mean in-flight value of the IMU accel high frequency vibration metric (rad/s) +imu_hfaccel_peak, Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s) +imu_hfaccel_mean, Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s) output_obs_ang_err_median, Median in-flight value of the output observer angular error (rad) output_obs_vel_err_median, Median in-flight value of the output observer velocity error (m/s) output_obs_pos_err_median, Median in-flight value of the output observer position error (m) diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index b4c10ac03fea..a22e7fec634f 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -11,7 +11,7 @@ from matplotlib.backends.backend_pdf import PdfPages from pyulog import ULog -from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags +from analysis.post_processing import magnetic_field_estimates_from_states, get_gps_check_fail_flags from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \ CheckFlagsPlot from analysis.detectors import PreconditionError @@ -33,6 +33,11 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str except: raise PreconditionError('could not find estimator_status instance', multi_instance) + try: + estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data + except: + raise PreconditionError('could not find estimator_status_flags instance', multi_instance) + try: estimator_states = ulog.get_dataset('estimator_states', multi_instance).data except: @@ -68,12 +73,13 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str except: raise PreconditionError('could not find innovation data') - control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) + gps_fail_flags = get_gps_check_fail_flags(estimator_status) status_time = 1e-6 * estimator_status['timestamp'] + status_flags_time = 1e-6 * estimator_status_flags['timestamp'] b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, \ - on_ground_transition_time = detect_airtime(control_mode, status_time) + on_ground_transition_time = detect_airtime(estimator_status_flags, status_flags_time) with PdfPages(output_plot_filename) as pdf_pages: @@ -173,9 +179,9 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot control mode summary A data_plot = ControlModeSummaryPlot( - status_time, control_mode, [['tilt_aligned', 'yaw_aligned'], - ['using_gps', 'using_optflow', 'using_evpos'], ['using_barohgt', 'using_gpshgt', - 'using_rnghgt', 'using_evhgt'], ['using_magyaw', 'using_mag3d', 'using_magdecl']], + status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'], + ['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt', + 'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']], x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'], annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding', 'external vision aiding'], ['Baro aiding', 'GPS aiding', 'rangefinder aiding', @@ -188,7 +194,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot control mode summary B # construct additional annotations for the airborne plot airborne_annotations = list() - if np.amin(np.diff(control_mode['airborne'])) > -0.5: + if np.amin(np.diff(estimator_status_flags['cs_in_air'])) > -0.5: airborne_annotations.append( (on_ground_transition_time, 'air to ground transition not detected')) else: @@ -197,7 +203,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str if in_air_duration > 0.0: airborne_annotations.append(((in_air_transition_time + on_ground_transition_time) / 2, 'duration = {:.1f} sec'.format(in_air_duration))) - if np.amax(np.diff(control_mode['airborne'])) < 0.5: + if np.amax(np.diff(estimator_status_flags['cs_in_air'])) < 0.5: airborne_annotations.append( (in_air_transition_time, 'ground to air transition not detected')) else: @@ -205,7 +211,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str (in_air_transition_time, 'in-air at {:.1f} sec'.format(in_air_transition_time))) data_plot = ControlModeSummaryPlot( - status_time, control_mode, [['airborne'], ['estimating_wind']], + status_flags_time, estimator_status_flags, [['cs_in_air'], ['cs_wind']], x_label='time (sec)', y_labels=['airborne', 'estimating wind'], annotation_text=[[], []], additional_annotation=[airborne_annotations, []], plot_title='EKF Control Status - Figure B', pdf_handle=pdf_pages) @@ -214,15 +220,15 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot innovation_check_flags summary data_plot = CheckFlagsPlot( - status_time, innov_flags, [['vel_innov_fail', 'posh_innov_fail'], ['posv_innov_fail', - 'hagl_innov_fail'], - ['magx_innov_fail', 'magy_innov_fail', 'magz_innov_fail', - 'yaw_innov_fail'], ['tas_innov_fail'], ['sli_innov_fail'], - ['ofx_innov_fail', - 'ofy_innov_fail']], x_label='time (sec)', + status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos', + 'reject_hagl'], + ['reject_mag_x', 'reject_mag_y', 'reject_mag_z', + 'reject_yaw'], ['reject_airspeed'], ['reject_sideslip'], + ['reject_optflow_x', + 'reject_optflow_y']], x_label='time (sec)', y_labels=['failed', 'failed', 'failed', 'failed', 'failed', 'failed'], y_lim=(-0.1, 1.1), - legend=[['vel NED', 'pos NE'], ['hgt absolute', 'hgt above ground'], + legend=[['vel NE', 'pos NE'], ['vel D', 'hgt absolute', 'hgt above ground'], ['mag_x', 'mag_y', 'mag_z', 'yaw'], ['airspeed'], ['sideslip'], ['flow X', 'flow Y']], plot_title='EKF Innovation Test Fails', annotate=False, pdf_handle=pdf_pages) @@ -250,18 +256,32 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str data_plot.save() data_plot.close() + # Plot the EKF IMU vibration metrics - scaled_estimator_status = {'vibe[0]': 1000. * estimator_status['vibe[0]'], - 'vibe[1]': 1000. * estimator_status['vibe[1]'], - 'vibe[2]': estimator_status['vibe[2]'] - } - data_plot = CheckFlagsPlot( - status_time, scaled_estimator_status, [['vibe[0]'], ['vibe[1]'], ['vibe[2]']], - x_label='time (sec)', y_labels=['Del Ang Coning (mrad)', 'HF Del Ang (mrad)', - 'HF Del Vel (m/s)'], plot_title='IMU Vibration Metrics', - pdf_handle=pdf_pages, annotate=True) - data_plot.save() - data_plot.close() + for imu_status_instance in range(4): + try: + vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data + + imu_status_time = 1e-6 * vehicle_imu_status_data['timestamp'] + + if vehicle_imu_status_data['accel_device_id'][0] == estimator_status['accel_device_id'][0]: + + scaled_estimator_status = {'delta_angle_coning_metric': 1000. * vehicle_imu_status_data['delta_angle_coning_metric'], + 'gyro_vibration_metric': vehicle_imu_status_data['gyro_vibration_metric'], + 'accel_vibration_metric': vehicle_imu_status_data['accel_vibration_metric'] + } + data_plot = CheckFlagsPlot( + imu_status_time, scaled_estimator_status, [['delta_angle_coning_metric'], ['gyro_vibration_metric'], ['accel_vibration_metric']], + x_label='time (sec)', + y_labels=['Del Ang Coning (mrad^2)', 'HF Gyro (rad/s)', 'HF accel (m/s/s)'], + plot_title='IMU Vibration Metrics', + pdf_handle=pdf_pages, annotate=True) + data_plot.save() + data_plot.close() + + except: + pass + # Plot the EKF output observer tracking errors scaled_innovations = { @@ -330,33 +350,33 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str data_plot.close() -def detect_airtime(control_mode, status_time): +def detect_airtime(estimator_status_flags, status_flags_time): # define flags for starting and finishing in air b_starts_in_air = False b_finishes_in_air = False # calculate in-air transition time - if (np.amin(control_mode['airborne']) < 0.5) and (np.amax(control_mode['airborne']) > 0.5): - in_air_transtion_time_arg = np.argmax(np.diff(control_mode['airborne'])) - in_air_transition_time = status_time[in_air_transtion_time_arg] - elif (np.amax(control_mode['airborne']) > 0.5): - in_air_transition_time = np.amin(status_time) + if (np.amin(estimator_status_flags['cs_in_air']) < 0.5) and (np.amax(estimator_status_flags['cs_in_air']) > 0.5): + in_air_transtion_time_arg = np.argmax(np.diff(estimator_status_flags['cs_in_air'])) + in_air_transition_time = status_flags_time[in_air_transtion_time_arg] + elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5): + in_air_transition_time = np.amin(status_flags_time) print('log starts while in-air at ' + str(round(in_air_transition_time, 1)) + ' sec') b_starts_in_air = True else: in_air_transition_time = float('NaN') print('always on ground') # calculate on-ground transition time - if (np.amin(np.diff(control_mode['airborne'])) < 0.0): - on_ground_transition_time_arg = np.argmin(np.diff(control_mode['airborne'])) - on_ground_transition_time = status_time[on_ground_transition_time_arg] - elif (np.amax(control_mode['airborne']) > 0.5): - on_ground_transition_time = np.amax(status_time) + if (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < 0.0): + on_ground_transition_time_arg = np.argmin(np.diff(estimator_status_flags['cs_in_air'])) + on_ground_transition_time = status_flags_time[on_ground_transition_time_arg] + elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5): + on_ground_transition_time = np.amax(status_flags_time) print('log finishes while in-air at ' + str(round(on_ground_transition_time, 1)) + ' sec') b_finishes_in_air = True else: on_ground_transition_time = float('NaN') print('always on ground') - if (np.amax(np.diff(control_mode['airborne'])) > 0.5) and (np.amin(np.diff(control_mode['airborne'])) < -0.5): + if (np.amax(np.diff(estimator_status_flags['cs_in_air'])) > 0.5) and (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < -0.5): if ((on_ground_transition_time - in_air_transition_time) > 0.0): in_air_duration = on_ground_transition_time - in_air_transition_time else: diff --git a/Tools/flightgear_bridge b/Tools/flightgear_bridge index 7c2c623da9f5..f47ce7b5fbbb 160000 --- a/Tools/flightgear_bridge +++ b/Tools/flightgear_bridge @@ -1 +1 @@ -Subproject commit 7c2c623da9f5dcb3f01d32830af1a2ed958de363 +Subproject commit f47ce7b5fbbb3aa43d33d2be1f6cd3746b13d5bf diff --git a/Tools/gazebo_sitl_multiple_run.sh b/Tools/gazebo_sitl_multiple_run.sh index dd051f28a49c..292272e2bd30 100755 --- a/Tools/gazebo_sitl_multiple_run.sh +++ b/Tools/gazebo_sitl_multiple_run.sh @@ -20,7 +20,7 @@ function spawn_model() { X=${X:=0.0} Y=${Y:=$((3*${N}))} - SUPPORTED_MODELS=("iris" "iris_rtps" "plane" "standard_vtol" "rover" "r1_rover" "typhoon_h480") + SUPPORTED_MODELS=("iris" "plane" "standard_vtol" "rover" "r1_rover" "typhoon_h480") if [[ " ${SUPPORTED_MODELS[*]} " != *"$MODEL"* ]]; then echo "ERROR: Currently only vehicle model $MODEL is not supported!" @@ -39,7 +39,7 @@ function spawn_model() { echo "Spawning ${MODEL}_${N} at ${X} ${Y}" - gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x ${X} -y ${Y} -z 0.0 + gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x ${X} -y ${Y} -z 0.83 popd &>/dev/null @@ -69,7 +69,7 @@ num_vehicles=${NUM_VEHICLES:=3} world=${WORLD:=empty} target=${TARGET:=px4_sitl_default} vehicle_model=${VEHICLE_MODEL:="iris"} -export PX4_SIM_MODEL=${vehicle_model}${LABEL} +export PX4_SIM_MODEL=${vehicle_model} echo ${SCRIPT} SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" @@ -86,8 +86,15 @@ sleep 1 source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target} +# To use gazebo_ros ROS2 plugins +if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then + ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so" +else + ros_args="" +fi + echo "Starting gazebo" -gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose & +gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose $ros_args & sleep 5 n=0 @@ -119,7 +126,8 @@ else m=0 while [ $m -lt ${target_number} ]; do - spawn_model ${target_vehicle} $n $target_x $target_y + export PX4_SIM_MODEL=${target_vehicle} + spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y m=$(($m + 1)) n=$(($n + 1)) done diff --git a/Tools/generate_board_targets_json.py b/Tools/generate_board_targets_json.py index 5dec7727dce4..25186fa4ecd9 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/generate_board_targets_json.py @@ -6,6 +6,14 @@ import sys import json import re +from kconfiglib import Kconfig + +kconf = Kconfig() + +# Supress warning output +kconf.warn_assign_undef = False +kconf.warn_assign_override = False +kconf.warn_assign_redun = False source_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..') @@ -28,42 +36,45 @@ 'uavcanv1' # TODO: fix and enable ] -def process_target(cmake_file, target_name): +def process_target(px4board_file, target_name): ret = None - is_board_def = False platform = None toolchain = None - for line in open(cmake_file, 'r'): - if 'px4_add_board' in line: - is_board_def = True - if not is_board_def: - continue - re_platform = re.search('PLATFORM\s+([^\s]+)', line) - if re_platform: platform = re_platform.group(1) - - re_toolchain = re.search('TOOLCHAIN\s+([^\s]+)', line) - if re_toolchain: toolchain = re_toolchain.group(1) - - if is_board_def: - assert platform, f"PLATFORM not found in {cmake_file}" - - if platform not in excluded_platforms: - # get the container based on the platform and toolchain - container = platform - if platform == 'posix': - container = 'base-focal' - if toolchain: - if toolchain.startswith('aarch64'): - container = 'aarch64' - elif toolchain == 'arm-linux-gnueabihf': - container = 'armhf' - else: - if verbose: print(f'possibly unmatched toolchain: {toolchain}') - elif platform == 'nuttx': - container = 'nuttx-focal' - - ret = {'target': target_name, 'container': container} + if px4board_file.endswith("default.px4board") or \ + px4board_file.endswith("recovery.px4board") or \ + 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) + kconf.load_config(default_kconfig, replace=True) + kconf.load_config(px4board_file, replace=False) + + if "BOARD_TOOLCHAIN" in kconf.syms: + toolchain = kconf.syms["BOARD_TOOLCHAIN"].str_value + + if "BOARD_PLATFORM" in kconf.syms: + platform = kconf.syms["BOARD_PLATFORM"].str_value + + assert platform, f"PLATFORM not found in {px4board_file}" + + if platform not in excluded_platforms: + # get the container based on the platform and toolchain + container = platform + if platform == 'posix': + container = 'base-focal' + if toolchain: + if toolchain.startswith('aarch64'): + container = 'aarch64' + elif toolchain == 'arm-linux-gnueabihf': + container = 'armhf' + else: + if verbose: print(f'possibly unmatched toolchain: {toolchain}') + elif platform == 'nuttx': + container = 'nuttx-focal' + + ret = {'target': target_name, 'container': container} + return ret for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): @@ -77,8 +88,8 @@ def process_target(cmake_file, target_name): if not board.is_dir(): continue for files in os.scandir(board.path): - if files.is_file() and files.name.endswith('.cmake'): - label = files.name[:-6] + if files.is_file() and files.name.endswith('.px4board'): + label = files.name[:-9] target_name = manufacturer.name + '_' + board.name + '_' + label if label in excluded_labels: if verbose: print(f'excluding label {label} ({target_name})') diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 358b6cca4093..66b764ada522 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 358b6cca4093646eb96e0cb075e45efa8f4a0c48 +Subproject commit 66b764ada522893c05224950aa6268c809f8e48a diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh index 06b9923189a8..0fae3982868f 100755 --- a/Tools/jmavsim_run.sh +++ b/Tools/jmavsim_run.sh @@ -5,12 +5,13 @@ set -e SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd "$SCRIPT_DIR/jMAVSim" -tcp_port=4560 +port=4560 extra_args= baudrate=921600 device= ip="127.0.0.1" -while getopts ":b:d:p:qsr:f:i:lo" opt; do +protocol="tcp" +while getopts ":b:d:u:p:qsr:f:i:loat" opt; do case $opt in b) baudrate=$OPTARG @@ -18,11 +19,14 @@ while getopts ":b:d:p:qsr:f:i:lo" opt; do d) device="$OPTARG" ;; + u) + protocol="udp" + ;; i) ip="$OPTARG" ;; p) - tcp_port=$OPTARG + port=$OPTARG ;; q) extra_args="$extra_args -qgc" @@ -39,6 +43,12 @@ while getopts ":b:d:p:qsr:f:i:lo" opt; do o) extra_args="$extra_args -disponly" ;; + a) + extra_args="$extra_args -fw" # aircraft model + ;; + t) + extra_args="$extra_args -ts" # tailsitter model + ;; \?) echo "Invalid option: -$OPTARG" >&2 exit 1 @@ -47,7 +57,11 @@ while getopts ":b:d:p:qsr:f:i:lo" opt; do done if [ "$device" == "" ]; then - device="-tcp $ip:$tcp_port" + if [ "$protocol" == "tcp" ]; then + device="-tcp $ip:$port" + else + device="-udp $port" + fi else device="-serial $device $baudrate" fi diff --git a/Tools/jsbsim_bridge b/Tools/jsbsim_bridge index e070c50d5915..68de2cc63ded 160000 --- a/Tools/jsbsim_bridge +++ b/Tools/jsbsim_bridge @@ -1 +1 @@ -Subproject commit e070c50d591541d440510b65c9da5cad1db5aa9a +Subproject commit 68de2cc63ded9a0d6641d45e9eb3ed2b43454cba diff --git a/Tools/kconfig/diffconfig.py b/Tools/kconfig/diffconfig.py new file mode 100755 index 000000000000..27e94f7402b7 --- /dev/null +++ b/Tools/kconfig/diffconfig.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# +# SPDX-License-Identifier: Apache-2.0 +# +# diffconfig - a tool to compare .config files. +# +# originally written in 2006 by Matt Mackall +# (at least, this was in his bloatwatch source code) +# last worked on 2008 by Tim Bird +# +# Source https://github.com/zephyrproject-rtos/zephyr/blob/zephyr-v2.3.0/scripts/kconfig/diffconfig +# +# Modified to include negated values during merge_style + +import sys, os + +def usage(): + print("""Usage: diffconfig [-h] [-m] [ ] + +Diffconfig is a simple utility for comparing two .config files. +Using standard diff to compare .config files often includes extraneous and +distracting information. This utility produces sorted output with only the +changes in configuration values between the two files. + +Added and removed items are shown with a leading plus or minus, respectively. +Changed items show the old and new values on a single line. + +If -m is specified, then output will be in "merge" style, which has the +changed and new values in kernel config option format. + +If no config files are specified, .config and .config.old are used. + +Example usage: + $ diffconfig .config config-with-some-changes +-EXT2_FS_XATTR n +-EXT2_FS_XIP n + CRAMFS n -> y + EXT2_FS y -> n + LOG_BUF_SHIFT 14 -> 16 + PRINTK_TIME n -> y +""") + sys.exit(0) + +# returns a dictionary of name/value pairs for config items in the file +def readconfig(config_file): + d = {} + for line in config_file: + line = line[:-1] + if line[:7] == "CONFIG_": + name, val = line[7:].split("=", 1) + d[name] = val + if line[-11:] == " is not set": + d[line[9:-11]] = "n" + return d + +def print_config(op, config, value, new_value): + global merge_style + + if merge_style: + if op=="-" and value=="y": + print("CONFIG_%s=n" % (config)) + elif op=="-" and not new_value: + print("# CONFIG_%s is not set" % (config)) + elif new_value: + if new_value=="n": + print("# CONFIG_%s is not set" % config) + else: + print("CONFIG_%s=%s" % (config, new_value)) + else: + if op=="-": + print("-%s %s" % (config, value)) + elif op=="+": + print("+%s %s" % (config, new_value)) + else: + print(" %s %s -> %s" % (config, value, new_value)) + +def main(merge, configa_filename, configb_filename): + global merge_style + + merge_style = merge + + try: + a = readconfig(open(configa_filename)) + b = readconfig(open(configb_filename)) + except (IOError): + e = sys.exc_info()[1] + print("I/O error[%s]: %s\n" % (e.args[0],e.args[1])) + usage() + + # print items in a but not b (accumulate, sort and print) + old = [] + for config in a: + if config not in b: + old.append(config) + old.sort() + for config in old: + print_config("-", config, a[config], None) + del a[config] + + # print items that changed (accumulate, sort, and print) + changed = [] + for config in a: + if a[config] != b[config]: + changed.append(config) + else: + del b[config] + changed.sort() + for config in changed: + print_config("->", config, a[config], b[config]) + del b[config] + + # now print items in b but not in a + # (items from b that were in a were removed above) + new = sorted(b.keys()) + for config in new: + print_config("+", config, None, b[config]) + +if __name__ == '__main__': + + # parse command line args + if ("-h" in sys.argv or "--help" in sys.argv): + usage() + + merge_style = 0 + if "-m" in sys.argv: + merge_style = 1 + sys.argv.remove("-m") + + argc = len(sys.argv) + if not (argc==1 or argc == 3): + print("Error: incorrect number of arguments or unrecognized option") + usage() + + if argc == 1: + # if no filenames given, assume .config and .config.old + build_dir="" + if "KBUILD_OUTPUT" in os.environ: + build_dir = os.environ["KBUILD_OUTPUT"]+"/" + configa_filename = build_dir + ".config.old" + configb_filename = build_dir + ".config" + else: + configa_filename = sys.argv[1] + configb_filename = sys.argv[2] + + main(merge_style, configa_filename, configb_filename) diff --git a/Tools/kconfig/merge_config.py b/Tools/kconfig/merge_config.py new file mode 100755 index 000000000000..eedae3cb6e0a --- /dev/null +++ b/Tools/kconfig/merge_config.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# This script functions similarly to scripts/kconfig/merge_config.sh from the +# kernel tree, merging multiple configurations fragments to produce a complete +# .config, with unspecified values filled in as for alldefconfig. +# +# The generated .config respects symbol dependencies, and a warning is printed +# if any symbol gets a different value from the assigned value. +# +# For a real-world merging example based on this script, see +# https://github.com/zephyrproject-rtos/zephyr/blob/master/scripts/kconfig/kconfig.py. +# +# Here's a demo: +# +# Kconfig contents: +# +# config FOO +# bool "FOO" +# +# config BAR +# bool "BAR" +# +# config BAZ +# string "BAZ" +# +# config QAZ +# bool "QAZ" if n +# +# +# conf1 contents: +# +# CONFIG_FOO=y +# +# +# conf2 contents: +# +# CONFIG_BAR=y +# +# +# conf3 contents: +# +# # Assigned twice (would generate warning if 'warn_assign_override' was +# # True) +# # CONFIG_FOO is not set +# +# # Ops... this symbol doesn't exist +# CONFIG_OPS=y +# +# CONFIG_BAZ="baz string" +# +# +# conf4 contents: +# +# CONFIG_QAZ=y +# +# +# Running: +# +# $ python(3) merge_config.py Kconfig merged conf1 conf2 conf3 conf4 +# Merged configuration 'conf1' +# Merged configuration 'conf2' +# conf3:5: warning: attempt to assign the value 'y' to the undefined symbol OPS +# Merged configuration 'conf3' +# Merged configuration 'conf4' +# Configuration saved to 'merged' +# warning: QAZ (defined at Kconfig:10) was assigned the value 'y' but got the value 'n' -- check dependencies +# $ cat merged +# Generated by Kconfiglib (https://github.com/ulfalizer/Kconfiglib) +# # CONFIG_FOO is not set +# CONFIG_BAR=y +# CONFIG_BAZ="baz string" + +from __future__ import print_function +import sys +import re +import pprint + +def _re_match(regex): + return re.compile(regex, re.ASCII).match + +from kconfiglib import Kconfig, BOOL, TRISTATE, TRI_TO_STR + +def main(kconfig_file, config1, config2): + + kconf = Kconfig(kconfig_file, suppress_traceback=True) + + # Enable warnings for assignments to undefined symbols + kconf.warn_assign_undef = False + + # (This script uses alldefconfig as the base. Other starting states could be + # set up here as well. The approach in examples/allnoconfig_simpler.py could + # provide an allnoconfig starting state for example.) + + # Disable warnings generated for multiple assignments to the same symbol within + # a (set of) configuration files. Assigning a symbol multiple times might be + # done intentionally when merging configuration files. + kconf.warn_assign_override = False + kconf.warn_assign_redun = False + + # Create a merged configuration by loading the fragments with replace=False. + # load_config() and write_config() returns a message to print. + print(kconf.load_config(config1, replace=False)) + print(kconf.load_config(config2, replace=False)) + + # Modification for PX4 unset all symbols (INT,HEX etc) from 2nd config + + f = open(config2, 'r') + + unset_match = re.compile(r"# {}([^ ]+) is not set".format("CONFIG_"), re.ASCII).match + + for line in f: + match = unset_match(line) + #pprint.pprint(line) + if match is not None: + sym_name = match.group(1) + kconf.syms[sym_name].unset_value() + + if kconf.syms[sym_name].type is BOOL: + for default, cond in kconf.syms[sym_name].orig_defaults: + if(cond.str_value == 'y'): + # Default is y, our diff is unset thus we've set it to no + kconf.syms[sym_name].set_value(0) + + f.close() + + # Print warnings for symbols whose actual value doesn't match the assigned + # value + for sym in kconf.defined_syms: + # Was the symbol assigned to? + if sym.user_value is not None: + # Tristate values are represented as 0, 1, 2. Having them as + # "n", "m", "y" is more convenient here, so convert. + if sym.type in (BOOL, TRISTATE): + user_value = TRI_TO_STR[sym.user_value] + else: + user_value = sym.user_value + + if user_value != sym.str_value: + print("warning: {} was assigned the value '{}' but got the " + "value '{}' -- check dependencies".format( + sym.name_and_loc, user_value, sym.str_value), + file=sys.stderr) + + return kconf + +if __name__ == '__main__': + if len(sys.argv) < 4: + sys.exit("usage: merge_config.py Kconfig merged_config config1 config2]") + # Write the merged configuration + print(main(sys.argv[1],sys.argv[3],sys.argv[4]).write_config(sys.argv[2])) diff --git a/Tools/kconfig/updateconfig.py b/Tools/kconfig/updateconfig.py new file mode 100755 index 000000000000..ee60f0234269 --- /dev/null +++ b/Tools/kconfig/updateconfig.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +############################################################################ +# +# 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. +# +############################################################################ + +""" + +import os +import glob +import kconfiglib +import tempfile +import sys + +import diffconfig +import merge_config + +px4_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../')) + +for name in glob.glob(px4_dir + '/boards/*/*/default.px4board'): + kconf = kconfiglib.Kconfig() + kconf.load_config(name) + print(kconf.write_min_config(name)) + +for name in glob.glob(px4_dir + '/boards/*/*/bootloader.px4board'): + kconf = kconfiglib.Kconfig() + kconf.load_config(name) + print(kconf.write_min_config(name)) + +for name in glob.glob(px4_dir + '/boards/*/*/canbootloader.px4board'): + kconf = kconfiglib.Kconfig() + kconf.load_config(name) + print(kconf.write_min_config(name)) + +for name in glob.glob(px4_dir + '/boards/*/*/*.px4board'): + if(os.path.basename(name) != "default.px4board" and + os.path.basename(name) != "bootloader.px4board" and + os.path.basename(name) != "canbootloader.px4board"): + board_default = os.path.dirname(name) + "/default.px4board"; + + # Merge with default config + kconf = merge_config.main(px4_dir + "/Kconfig", board_default, name) + tf = tempfile.NamedTemporaryFile() + + # Save minconfig + kconf.write_min_config(tf.name) + + # Diff with default config and save to label.px4board + stdoutpipe = sys.stdout + sys.stdout = open(name, "w") + diffconfig.main(1, board_default, tf.name) + sys.stdout.close() + sys.stdout = stdoutpipe diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py index b59d140417d7..35c8d130ed5a 100755 --- a/Tools/mavlink_shell.py +++ b/Tools/mavlink_shell.py @@ -12,6 +12,7 @@ import termios from timeit import default_timer as timer from argparse import ArgumentParser +import os try: from pymavlink import mavutil @@ -44,6 +45,7 @@ def __init__(self, portname, baudrate, devnum=0, debug=0): self.port = devnum self.debug("Connecting with MAVLink to %s ..." % portname) self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate) + self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GENERIC, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) self.mav.wait_heartbeat() self.debug("HEARTBEAT OK\n") self.debug("Locked serial device\n") @@ -135,15 +137,20 @@ def main(): mav_serialport.write('\n') # make sure the shell is started - # setup the console, so we can read one char at a time + # disable echo & avoid buffering on stdin fd_in = sys.stdin.fileno() - old_attr = termios.tcgetattr(fd_in) - new_attr = termios.tcgetattr(fd_in) - new_attr[3] = new_attr[3] & ~termios.ECHO # lflags - new_attr[3] = new_attr[3] & ~termios.ICANON - try: + old_attr = termios.tcgetattr(fd_in) + new_attr = termios.tcgetattr(fd_in) + new_attr[3] = new_attr[3] & ~termios.ECHO # lflags + new_attr[3] = new_attr[3] & ~termios.ICANON termios.tcsetattr(fd_in, termios.TCSANOW, new_attr) + except termios.error: + # tcgetattr can fail if stdin is not a tty + old_attr = None + ubuf_stdin = os.fdopen(fd_in, 'rb', buffering=0) + + try: cur_line = '' command_history = [] cur_history_index = 0 @@ -156,11 +163,19 @@ def erase_last_n_chars(N): next_heartbeat_time = timer() - while True: + quit_time = None + while quit_time is None or quit_time > timer(): while True: - i, o, e = select.select([sys.stdin], [], [], 0) + i, o, e = select.select([ubuf_stdin], [], [], 0) if not i: break - ch = sys.stdin.read(1) + ch = ubuf_stdin.read(1).decode('utf8') + + if len(ch) == 0: # EOF + if quit_time is None: + # run a bit longer to read the response (we could also + # read until we get a prompt) + quit_time = timer() + 1 + break # provide a simple shell with command history if ch == '\n': @@ -182,8 +197,8 @@ def erase_last_n_chars(N): cur_line = cur_line[:-1] sys.stdout.write(ch) elif ord(ch) == 27: - ch = sys.stdin.read(1) # skip one - ch = sys.stdin.read(1) + ch = ubuf_stdin.read(1).decode('utf8') # skip one + ch = ubuf_stdin.read(1).decode('utf8') if ch == 'A': # arrow up if cur_history_index > 0: cur_history_index -= 1 @@ -212,8 +227,7 @@ def erase_last_n_chars(N): # handle heartbeat sending heartbeat_time = timer() if heartbeat_time > next_heartbeat_time: - mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, - mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0) + mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GENERIC, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0) next_heartbeat_time = heartbeat_time + 1 except serial.serialutil.SerialException as e: @@ -223,7 +237,8 @@ def erase_last_n_chars(N): mav_serialport.close() finally: - termios.tcsetattr(fd_in, termios.TCSADRAIN, old_attr) + if old_attr: + termios.tcsetattr(fd_in, termios.TCSADRAIN, old_attr) if __name__ == '__main__': diff --git a/Tools/mavlink_ulog_streaming.py b/Tools/mavlink_ulog_streaming.py index ffd32f1c7fad..c0ff6fe15dc3 100755 --- a/Tools/mavlink_ulog_streaming.py +++ b/Tools/mavlink_ulog_streaming.py @@ -13,6 +13,7 @@ from timeit import default_timer as timer os.environ['MAVLINK20'] = '1' # The commands require mavlink 2 from argparse import ArgumentParser +import signal try: from pymavlink import mavutil @@ -24,6 +25,8 @@ print("") sys.exit(1) +class LoggingCompleted(Exception): + pass class MavlinkLogStreaming(): '''Streams log data via MAVLink. @@ -49,6 +52,7 @@ def __init__(self, portname, baudrate, output_filename, debug=0): self.logging_started = False self.num_dropouts = 0 self.target_component = 1 + self.got_sig_int = False def debug(self, s, level=1): '''write some debug text''' @@ -67,13 +71,24 @@ def stop_log(self): mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0, 0, 0, 0, 0, 0, 0, 0) + def _int_handler(self, sig, frame): + self.got_sig_int = True + def read_messages(self): ''' main loop reading messages ''' measure_time_start = timer() measured_data = 0 next_heartbeat_time = timer() + old_handler = signal.signal(signal.SIGINT, self._int_handler) + while True: + if self.got_sig_int: + signal.signal(signal.SIGINT, old_handler) + self.got_sig_int = False + print('\nStopping log...') + self.stop_log() + # Continue reading until we get an ACK # handle heartbeat sending heartbeat_time = timer() @@ -120,6 +135,9 @@ def read_message(self): print('Logging started. Waiting for Header...') else: raise Exception('Logging start failed', m.result) + elif m.command == mavutil.mavlink.MAV_CMD_LOGGING_STOP and \ + m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED: + raise LoggingCompleted() return None, 0, 0 # m is either 'LOGGING_DATA_ACKED' or 'LOGGING_DATA': @@ -137,7 +155,8 @@ def read_message(self): if m.get_type() == 'LOGGING_DATA': if not self.got_header_section: - print('Header received in {:0.2f}s'.format(timer()-self.start_time)) + print('Header received in {:0.2f}s (size: {:.1f} KB)'.format( + timer()-self.start_time, self.file.tell()/1024)) self.logging_started = True self.got_header_section = True self.last_sequence = m.sequence @@ -256,14 +275,10 @@ def main(): print('Starting log...') mav_log_streaming.start_log() mav_log_streaming.read_messages() - - print('Stopping log') - mav_log_streaming.stop_log() - except KeyboardInterrupt: - print('Stopping log') - mav_log_streaming.stop_log() - + print('Aborting') + except LoggingCompleted: + print('Done') if __name__ == '__main__': main() diff --git a/Tools/module_config/generate_actuators_metadata.py b/Tools/module_config/generate_actuators_metadata.py new file mode 100755 index 000000000000..a7348be7c11b --- /dev/null +++ b/Tools/module_config/generate_actuators_metadata.py @@ -0,0 +1,496 @@ +#!/usr/bin/env python3 +""" Script to generate actuators.json metadata from module.yaml config file(s) +""" + +import argparse +import lzma #to create .xz file +import json +import os +import sys + +from output_groups_from_timer_config import get_timer_groups, get_output_groups + +try: + import yaml +except ImportError as e: + print("Failed to import yaml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyyaml") + print("") + sys.exit(1) + +parser = argparse.ArgumentParser(description='Generate actuators.json from module.yaml file(s)') + +parser.add_argument('--config-files', type=str, nargs='*', default=[], + help='YAML module config file(s)') +parser.add_argument('--output-file', type=str, action='store', + help='JSON output file', required=True) +parser.add_argument('--compress', action='store_true', help='Add a compressed output file') +parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', + help='Verbose Output') +parser.add_argument('--timer-config', type=str, action='store', + help='board-specific timer_config.cpp file') +parser.add_argument('--board', type=str, action='store', + help='board name, e.g. ') +parser.add_argument('--board-with-io', dest='board_with_io', action='store_true', + help='Indicate that the board as an IO for extra PWM', + default=False) + +args = parser.parse_args() + +compress = args.compress +verbose = args.verbose +output_file = args.output_file +timer_config_file = args.timer_config +board_with_io = args.board_with_io +board = args.board + +root_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../..") +output_functions_file = os.path.join(root_dir,"src/lib/mixer_module/output_functions.yaml") + +def save_compressed(filename): + #create lzma compressed version + xz_filename=filename+'.xz' + with lzma.open(xz_filename, 'wt', preset=9) as f: + with open(filename, 'r') as content_file: + f.write(content_file.read()) + +def load_yaml_file(file_name): + with open(file_name, 'r') as stream: + try: + return yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + raise + +# functions +output_functions_yaml = load_yaml_file(output_functions_file) +output_functions = output_functions_yaml['functions'] +functions = {} + +def add_function(functions, index, name, function_obj=None): + functions[index] = { + "label": name + } + if function_obj is not None: + if function_obj.get('exclude_from_actuator_testing', False): + functions[index]['exclude-from-actuator-testing'] = True + if 'note' in function_obj: + functions[index]['note'] = function_obj['note'] + +for group_key in output_functions: + group = output_functions[group_key] + for function_name in group: + function_name_label = function_name.replace('_', ' ') + if isinstance(group[function_name], int): + add_function(functions, group[function_name], function_name_label) + elif not 'count' in group[function_name]: + add_function(functions, group[function_name]['start'], function_name_label, group[function_name]) + else: + start = group[function_name]['start'] + count = group[function_name]['count'] + for i in range(count): + add_function(functions, start+i, function_name_label+' '+str(i+1), group[function_name]) + +# outputs +outputs = [] + +def process_module_name(module_name): + if module_name == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'PWM AUX' + return 'PWM MAIN' + if '${' in module_name: + raise Exception('unhandled variable in {:}'.format(module_name)) + return module_name + +def process_param_prefix(param_prefix): + if param_prefix == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'PWM_AUX' + return 'PWM_MAIN' + if '${' in param_prefix: + raise Exception('unhandled variable in {:}'.format(param_prefix)) + return param_prefix + +def process_channel_label(module_name, channel_label, no_prefix): + if channel_label == '${PWM_MAIN_OR_AUX_CAP}': + return 'CAP' + if channel_label == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'AUX' + return 'MAIN' + if '${' in channel_label: + raise Exception('unhandled variable in {:}'.format(channel_label)) + if no_prefix: return channel_label + return channel_label + +def get_actuator_output(yaml_config, output_functions, timer_config_file, verbose): + """ parse the actuator_output section from the yaml config file + """ + if not 'actuator_output' in yaml_config: + return None + + + actuator_output_yaml = yaml_config['actuator_output'] + output_groups = actuator_output_yaml['output_groups'] + module_name = process_module_name(yaml_config['module_name']) + group_idx = 0 + + if verbose: print('processing module: {}'.format(module_name)) + + actuator_output = { + 'label': module_name + } + if 'show_subgroups_if' in actuator_output_yaml: + actuator_output['show-subgroups-if'] = actuator_output_yaml['show_subgroups_if'] + + # config parameters + def get_config_params(param_list): + """ convert config parameter list (per group or per subgroup) """ + parameters = [] + for config_param in param_list: + if verbose: + print('config param: {}'.format(config_param)) + param = { + 'name': config_param['param'], + } + if 'label' in config_param: + param['label'] = config_param['label'] + if 'function' in config_param: + param['function'] = config_param['function'] + parameters.append(param) + return parameters + + parameters = get_config_params(actuator_output_yaml.get('config_parameters', [])) + if len(parameters) > 0: + actuator_output['parameters'] = parameters + + subgroups = [] + + while group_idx < len(output_groups): + group = output_groups[group_idx] + group_idx += 1 + + if verbose: print("processing group: {:}".format(group)) + + # Check for generator and generate additional data. + if 'generator' in group: + if group['generator'] == 'pwm': + param_prefix = process_param_prefix(group['param_prefix']) + no_prefix = not group.get('channel_label_module_name_prefix', True) + channel_labels = [process_channel_label(module_name, label, no_prefix) + for label in group['channel_labels']] + standard_params = group.get('standard_params', []) + extra_function_groups = group.get('extra_function_groups', []) + pwm_timer_param = group.get('pwm_timer_param', None) + if 'timer_config_file' in group: + timer_config_file = os.path.join(root_dir, group['timer_config_file']) + if timer_config_file is None: + raise Exception('trying to generate pwm outputs, but --timer-config not set') + timer_groups = get_timer_groups(timer_config_file, verbose) + timer_output_groups, timer_params = get_output_groups(timer_groups, + param_prefix, channel_labels, + standard_params, extra_function_groups, pwm_timer_param, + verbose=verbose) + output_groups.extend(timer_output_groups) + else: + raise Exception('unknown generator {:}'.format(group['generator'])) + continue + + subgroup = {} + + # supported actions + if 'supported_actions' in group: + actions = {} + for action_name in group['supported_actions']: + action = group['supported_actions'][action_name] + action_name = action_name.replace('_', '-') + actions[action_name] = {} + if 'supported_if' in action: + actions[action_name]['supported-if'] = action['supported_if'] + if 'actuator_types' in action: + actions[action_name]['actuator-types'] = action['actuator_types'] + subgroup['supported-actions'] = actions + + # channels + num_channels = group['num_channels'] + no_prefix = not group.get('channel_label_module_name_prefix', True) + channel_label = process_channel_label(module_name, group['channel_label'], no_prefix) + instance_start = group.get('instance_start', 1) + instance_start_label = group.get('instance_start_label', instance_start) + channels = [] + for channel in range(num_channels): + channels.append({ + 'label': channel_label + ' ' +str(channel+instance_start_label), + 'param-index': channel+instance_start + }) + subgroup['channels'] = channels + + if 'group_label' in group: + subgroup['label'] = group['group_label'] + + + # per-channel-params + per_channel_params = [] + param_prefix = process_param_prefix(group['param_prefix']) + standard_params = group.get('standard_params', {}) + standard_params_array = [ + ( 'function', 'Function', 'FUNC', False ), + ( 'disarmed', 'Disarmed', 'DIS', False ), + ( 'min', 'Minimum', 'MIN', False ), + ( 'max', 'Maximum', 'MAX', False ), + ( 'failsafe', 'Failsafe', 'FAIL', True ), + ] + for key, label, param_suffix, advanced in standard_params_array: + show_if = None + if key in standard_params and 'show_if' in standard_params[key]: + show_if = standard_params[key]['show_if'] + + if key in standard_params or key == 'function': + param = { + 'label': label, + 'name': param_prefix+'_'+param_suffix+'${i}', + 'function': key, + } + if advanced: param['advanced'] = True + if show_if: param['show-if'] = show_if + per_channel_params.append(param) + + + param = { + 'label': 'Rev Range\n(for Servos)', + 'name': param_prefix+'_REV', + 'index-offset': -1, + 'show-as': 'bitset', + } + per_channel_params.append(param) + + # TODO: support non-standard per-channel parameters + + subgroup['per-channel-parameters'] = per_channel_params + + # group config params + parameters = get_config_params(group.get('config_parameters', [])) + if len(parameters) > 0: + subgroup['parameters'] = parameters + + subgroups.append(subgroup) + + actuator_output['subgroups'] = subgroups + return actuator_output + +# Mixers +mixers = None +def get_mixers(yaml_config, output_functions, verbose): + if not 'mixer' in yaml_config: + return None + + actuator_types = {} + for actuator_type_key in yaml_config['mixer']['actuator_types']: + actuator_type_conf = yaml_config['mixer']['actuator_types'][actuator_type_key] + actuator_type = { } + if actuator_type_key != 'DEFAULT': + actuator_type['label-index-offset'] = 1 # always 1 + if 'functions' in actuator_type_conf: + function_name = actuator_type_conf['functions'] + # we expect the function to be in 'common' (this is not a requirement, just simplicity) + output_function = output_functions['common'][function_name] + actuator_type['function-min'] = output_function['start'] + actuator_type['function-max'] = output_function['start'] + output_function['count'] - 1 + + values = actuator_type_conf['actuator_testing_values'] + actuator_type['values'] = { + 'min': values['min'], + 'max': values['max'], + } + if values.get('default_is_nan', False): + actuator_type['values']['default-is-nan'] = True + else: + actuator_type['values']['default'] = values['default'] + if values.get('reversible', False): + actuator_type['values']['reversible'] = True + + # per item params + per_item_params = [] + for per_item_param in actuator_type_conf.get('per_item_parameters', []): + per_item_params.append({k.replace('_','-'): v for k, v in per_item_param.items()}) + if len(per_item_params) > 0: + actuator_type['per-item-parameters'] = per_item_params + + actuator_types[actuator_type_key] = actuator_type + + if verbose: + print('Actuator types: {}'.format(actuator_types)) + + config = [] + yaml_mixer_config = yaml_config['mixer']['config'] + select_param = yaml_mixer_config['param'] + types = yaml_mixer_config['types'] + for type_index in types: + current_type = types[type_index] + option = select_param + '==' + str(type_index) + mixer_config = { + 'option': option, + 'help-url': 'https://docs.px4.io/master/en/config/actuators.html', + } + for optional in ['type', 'title']: + if optional in current_type: + mixer_config[optional] = current_type[optional] + actuators = [] + for actuator_conf in current_type['actuators']: + actuator = { + 'actuator-type': actuator_conf['actuator_type'], + 'required': True, # for now always set as required + } + # sanity check that actuator type exists + if actuator_conf['actuator_type'] not in actuator_types: + raise Exception('actuator type "{}" does not exist (valid: {})'.format(actuator_conf['actuator_type'], actuator_types.keys())) + + if 'group_label' in actuator_conf: + actuator['group-label'] = actuator_conf['group_label'] + else: + # infer from actuator type + if actuator_conf['actuator_type'] == 'motor': + actuator['group-label'] = 'Motors' + elif actuator_conf['actuator_type'] == 'servo': + actuator['group-label'] = 'Servos' + else: + raise Exception('Missing group label for actuator type "{}"'.format(actuator_conf['actuator_type'])) + + if 'count' in actuator_conf: # possibly dynamic size + actuator['count'] = actuator_conf['count'] + per_item_params = actuator_conf.get('per_item_parameters', {}) + params = [] + if 'standard' in per_item_params: + standard_params = per_item_params['standard'] + index_offset = standard_params.get('index_offset', 0) + if 'position' in standard_params: + params.extend([ + { + 'label': 'Position X', + 'function': 'posx', + 'name': standard_params['position'][0], + 'index-offset': index_offset, + }, + { + 'label': 'Position Y', + 'function': 'posy', + 'name': standard_params['position'][1], + 'index-offset': index_offset, + }, + { + 'label': 'Position Z', + 'function': 'posz', + 'name': standard_params['position'][2], + 'advanced': True, + 'index-offset': index_offset, + }, + ]) + if 'extra' in per_item_params: + for extra_param in per_item_params['extra']: + params.append({k.replace('_','-'): v for k, v in extra_param.items()}) + actuator['per-item-parameters'] = params + if 'item_label_prefix' in actuator_conf: + actuator['item-label-prefix'] = actuator_conf['item_label_prefix'] + else: # fixed size + labels = [] + pos_x = [] + pos_y = [] + pos_z = [] + for instance in actuator_conf['instances']: + labels.append(instance['name']) + pos_x.append(instance['position'][0]) + pos_y.append(instance['position'][1]) + pos_z.append(instance['position'][2]) + actuator['count'] = len(labels) + actuator['item-label-prefix'] = labels + actuator['per-item-parameters'] = [ + { + 'label': 'Position X', + 'function': 'posx', + 'value': pos_x, + }, + { + 'label': 'Position Y', + 'function': 'posy', + 'value': pos_y, + }, + { + 'label': 'Position Z', + 'function': 'posz', + 'value': pos_z, + 'advanced': True, + }, + ] + + # actuator parameters + parameters = [] + for param in actuator_conf.get('parameters', []): + parameters.append({k.replace('_','-'): v for k, v in param.items()}) + actuator['parameters'] = parameters + + actuators.append(actuator) + + mixer_config['actuators'] = actuators + config.append(mixer_config) + + if verbose: + print('Mixer configs: {}'.format(config)) + + rules = [] + for rule in yaml_config['mixer'].get('rules', []): + rules.append({k.replace('_','-'): v for k, v in rule.items()}) + + if verbose: + print('Mixer rules: {}'.format(rules)) + + mixers = { + 'actuator-types': actuator_types, + 'config': config, + 'rules': rules, + } + return mixers + + +for yaml_file in args.config_files: + yaml_config = load_yaml_file(yaml_file) + + try: + actuator_output = get_actuator_output(yaml_config, + output_functions, timer_config_file, verbose) + if actuator_output: + outputs.append(actuator_output) + + parsed_mixers = get_mixers(yaml_config, output_functions, verbose) + if parsed_mixers is not None: + if mixers is not None: + # only expected to be configured in one module + raise Exception('multiple "mixer" sections in module config files') + mixers = parsed_mixers + except Exception as e: + print('Exception while parsing {:}:'.format(yaml_file)) + raise e + +if mixers is None: + if len(outputs) > 0: + raise Exception('Missing "mixer" section in yaml configs (CONFIG_MODULES_CONTROL_ALLOCATOR not added to the build?)') + else: + # set a minimal default + mixers = { + 'actuator-types': { 'DEFAULT': { 'values': { 'min': 0, 'max': 1 } } }, + 'config': [], + } + +actuators = { + 'version': 1, + 'show-ui-if': 'SYS_CTRL_ALLOC==1', + 'outputs_v1': outputs, + 'functions_v1': functions, + 'mixer_v1': mixers, + } + +with open(output_file, 'w') as outfile: + indent = 2 if verbose else None + json.dump(actuators, outfile, indent=indent) + +if compress: + save_compressed(output_file) diff --git a/Tools/module_config/generate_params.py b/Tools/module_config/generate_params.py new file mode 100755 index 000000000000..68a9689ac3d9 --- /dev/null +++ b/Tools/module_config/generate_params.py @@ -0,0 +1,405 @@ +#!/usr/bin/env python3 +""" Script to generate params from module.yaml config file(s) + Note: serial params are handled in Tools/serial/generate_config.py +""" + +import argparse +import os +import sys +from copy import deepcopy + +from output_groups_from_timer_config import get_timer_groups, get_output_groups + +try: + import yaml +except ImportError as e: + print("Failed to import yaml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyyaml") + print("") + sys.exit(1) + +parser = argparse.ArgumentParser(description='Generate params from module.yaml file(s)') + +parser.add_argument('--config-files', type=str, nargs='*', default=[], + help='YAML module config file(s)') +parser.add_argument('--params-file', type=str, action='store', + help='Parameter output file') +parser.add_argument('--timer-config', type=str, action='store', + help='board-specific timer_config.cpp file') +parser.add_argument('--ethernet', action='store_true', + help='Ethernet support') +parser.add_argument('--board', type=str, action='store', + help='board name, e.g. ') +parser.add_argument('--board-with-io', dest='board_with_io', action='store_true', + help='Indicate that the board as an IO for extra PWM', + default=False) +parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', + help='Verbose Output') + +args = parser.parse_args() + +verbose = args.verbose +params_output_file = args.params_file +timer_config_file = args.timer_config +ethernet_supported = args.ethernet +board_with_io = args.board_with_io +board = args.board + +root_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../..") +output_functions_file = os.path.join(root_dir,"src/lib/mixer_module/output_functions.yaml") + +def process_module_name(module_name): + if module_name == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'PWM AUX' + return 'PWM MAIN' + if '${' in module_name: + raise Exception('unhandled variable in {:}'.format(module_name)) + return module_name + +def process_param_prefix(param_prefix): + if param_prefix == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'PWM_AUX' + return 'PWM_MAIN' + if '${' in param_prefix: + raise Exception('unhandled variable in {:}'.format(param_prefix)) + return param_prefix + +def process_channel_label(module_name, channel_label, no_prefix): + if channel_label == '${PWM_MAIN_OR_AUX_CAP}': + return 'PWM Capture' + if channel_label == '${PWM_MAIN_OR_AUX}': + if board_with_io: return 'PWM Aux' + return 'PWM Main' + if '${' in channel_label: + raise Exception('unhandled variable in {:}'.format(channel_label)) + if no_prefix: return channel_label + return module_name + ' ' + channel_label + + +def parse_yaml_parameters_config(yaml_config, ethernet_supported): + """ parse the parameters section from the yaml config file """ + if 'parameters' not in yaml_config: + return '' + parameters_section_list = yaml_config['parameters'] + ret = '' + for parameters_section in parameters_section_list: + if 'definitions' not in parameters_section: + continue + definitions = parameters_section['definitions'] + param_group = parameters_section.get('group', None) + for param_name in definitions: + # 'definitions' either contains the param definition directly (dict), + # or a list of definitions with that name (multiple entries for a + # multi-instance param with different instance_start) + param_list = definitions[param_name] + if not isinstance(param_list, list): + param_list = [param_list] + + for param in param_list: + if param.get('requires_ethernet', False) and not ethernet_supported: + continue + num_instances = param.get('num_instances', 1) + instance_start = param.get('instance_start', 0) # offset + instance_start_label = param.get('instance_start_label', instance_start) + + # get the type and extract all tags + tags = '@group {:}'.format(param_group) + if param['type'] == 'enum': + param_type = 'INT32' + for key in param['values']: + tags += '\n * @value {:} {:}'.format(key, param['values'][key]) + elif param['type'] == 'bitmask': + param_type = 'INT32' + for key in param['bit']: + tags += '\n * @bit {:} {:}'.format(key, param['bit'][key]) + max_val = max(key for key in param['bit']) + tags += '\n * @min 0' + tags += '\n * @max {:}'.format((1<<(max_val+1)) - 1) + elif param['type'] == 'boolean': + param_type = 'INT32' + tags += '\n * @boolean' + elif param['type'] == 'int32': + param_type = 'INT32' + elif param['type'] == 'float': + param_type = 'FLOAT' + else: + raise Exception("unknown param type {:}".format(param['type'])) + + for tag in ['decimal', 'increment', 'category', 'volatile', + 'min', 'max', 'unit', 'reboot_required']: + if tag in param: + tags += '\n * @{:} {:}'.format(tag, param[tag]) + + for i in range(num_instances): + # default value + default_value = 0 + if 'default' in param: + # default can be a list of num_instances or a single value + if type(param['default']) == list: + assert len(param['default']) == num_instances + default_value = param['default'][i] + else: + default_value = param['default'] + + if type(default_value) == bool: + default_value = int(default_value) + + # output the existing C-style format + ret += ''' +/** + * {short_descr} + * + * {long_descr} + * + * {tags} + */ +PARAM_DEFINE_{param_type}({name}, {default_value}); +'''.format(short_descr=param['description']['short'].replace("\n", "\n * "), + long_descr=param['description'].get('long', "").replace("\n", "\n * "), + tags=tags, + param_type=param_type, + name=param_name.replace('${i}', str(i+instance_start)), + default_value=default_value, + ).replace('${i}', str(i+instance_start_label)) + return ret + + +def get_actuator_output_params(yaml_config, output_functions, + timer_config_file, verbose): + """ parse the actuator_output section from the yaml config file + :return: dict of param definitions + """ + if not 'actuator_output' in yaml_config: + return {} + output_groups = yaml_config['actuator_output']['output_groups'] + module_name = process_module_name(yaml_config['module_name']) + all_params = {} + group_idx = 0 + + all_param_prefixes = {} + + def add_local_param(param_name, param_def): + nonlocal all_params + # add as a list, as there can be multiple entries with the same param_name + if not param_name in all_params: + all_params[param_name] = [] + all_params[param_name].append(param_def) + + while group_idx < len(output_groups): + group = output_groups[group_idx] + group_idx += 1 + + if verbose: print("processing group: {:}".format(group)) + + # Check for generator and generate additional data. + # We do this by extending the output_groups list and parse in a later iteration + if 'generator' in group: + if group['generator'] == 'pwm': + param_prefix = process_param_prefix(group['param_prefix']) + no_prefix = not group.get('channel_label_module_name_prefix', True) + channel_labels = [process_channel_label(module_name, label, no_prefix) + for label in group['channel_labels']] + standard_params = group.get('standard_params', []) + extra_function_groups = group.get('extra_function_groups', []) + pwm_timer_param = group.get('pwm_timer_param', None) + if 'timer_config_file' in group: + timer_config_file = os.path.join(root_dir, group['timer_config_file']) + if timer_config_file is None: + raise Exception('trying to generate pwm outputs, but --timer-config not set') + timer_groups = get_timer_groups(timer_config_file, verbose) + timer_output_groups, timer_params = get_output_groups(timer_groups, + param_prefix, channel_labels, + standard_params, extra_function_groups, pwm_timer_param, + verbose=verbose) + all_params.update(timer_params) + output_groups.extend(timer_output_groups) + + # In case of a board w/o IO and >8 PWM channels, pwm_out splits + # into 2 instances (if SYS_CTRL_ALLOC==0) and we need to add the + # PWM_AUX min/max/disarmed params as well. + num_channels = len(timer_groups['types']) + if not board_with_io and num_channels > 8: + output_groups.append( + { + 'param_prefix': 'PWM_AUX', + 'channel_label': 'PWM AUX', + 'instance_start': 1, + 'num_channels': num_channels - 8, + 'standard_params': deepcopy(timer_output_groups[0]['standard_params']) + }) + + else: + raise Exception('unknown generator {:}'.format(group['generator'])) + continue + + num_channels = group['num_channels'] + param_prefix = process_param_prefix(group['param_prefix']) + no_prefix = not group.get('channel_label_module_name_prefix', True) + channel_label = process_channel_label(module_name, group['channel_label'], no_prefix) + standard_params = group.get('standard_params', {}) + instance_start = group.get('instance_start', 1) + instance_start_label = group.get('instance_start_label', instance_start) + if len(param_prefix) > 9: # 16 - len('_FAIL') - 2 (2 digits for index) + raise Exception("param prefix {:} too long (max length=10)".format(param_prefix)) + # collect the functions + function_groups = ['common'] + function_groups.extend(group.get('extra_function_groups', [])) + output_function_values = {} + for function_group in function_groups: + group = output_functions[function_group] + for function_name in group: + function_name_label = function_name.replace('_', ' ') + if isinstance(group[function_name], int): + output_function_values[group[function_name]] = function_name_label + elif not 'count' in group[function_name]: + output_function_values[group[function_name]['start']] = function_name_label + else: + start = group[function_name]['start'] + count = group[function_name]['count'] + for i in range(count): + output_function_values[start+i] = function_name_label+' '+str(i+1) + + if param_prefix not in all_param_prefixes: + all_param_prefixes[param_prefix] = [] + all_param_prefixes[param_prefix].append((instance_start, + instance_start_label, num_channels, channel_label)) + + # function param + param = { + 'description': { + 'short': channel_label+' ${i} Output Function', + 'long': +'''Select what should be output on {:} ${{i}}. + +The default failsafe value is set according to the selected function: +- 'Min' for ConstantMin +- 'Max' for ConstantMax +- 'Max' for Parachute +- ('Max'+'Min')/2 for Servos +- 'Disarmed' for the rest +'''.format(channel_label), + }, + 'type': 'enum', + 'instance_start': instance_start, + 'instance_start_label': instance_start_label, + 'num_instances': num_channels, + 'default': 0, + 'values': output_function_values + } + add_local_param(param_prefix+'_FUNC${i}', param) + + # handle standard_params + disarmed_description = \ +'''This is the output value that is set when not armed. + +Note that non-motor outputs might already be active in prearm state if COM_PREARM_MODE is set. +''' + minimum_description = \ +'''Minimum output value (when not disarmed). +''' + maximum_description = \ +'''Maxmimum output value (when not disarmed). +''' + failsafe_description = \ +'''This is the output value that is set when in failsafe mode. + +When set to -1 (default), the value depends on the function (see {:}). +'''.format(param_prefix+'_FUNC${i}') + standard_params_array = [ + ( 'disarmed', 'Disarmed', 'DIS', disarmed_description ), + ( 'min', 'Minimum', 'MIN', minimum_description ), + ( 'max', 'Maximum', 'MAX', maximum_description ), + ( 'failsafe', 'Failsafe', 'FAIL', failsafe_description ), + ] + for key, label, param_suffix, description in standard_params_array: + if key in standard_params: + + # values must be in range of an uint16_t + if standard_params[key]['min'] < 0: + raise Exception('minimum value for {:} expected >= 0 (got {:})'.format(key, standard_params[key]['min'])) + if standard_params[key]['max'] >= 1<<16: + raise Exception('maximum value for {:} expected <= {:} (got {:})'.format(key, 1<<16, standard_params[key]['max'])) + + if key == 'failsafe': + standard_params[key]['default'] = -1 + standard_params[key]['min'] = -1 + + param = { + 'description': { + 'short': channel_label+' ${i} '+label+' Value', + 'long': description + }, + 'type': 'int32', + 'instance_start': instance_start, + 'instance_start_label': instance_start_label, + 'num_instances': num_channels, + 'min': standard_params[key]['min'], + 'max': standard_params[key]['max'], + 'default': standard_params[key]['default'], + } + add_local_param(param_prefix+'_'+param_suffix+'${i}', param) + + # add reverse range param + for param_prefix in all_param_prefixes: + groups = all_param_prefixes[param_prefix] + # collect the bits + channel_bits = {} + for instance_start, instance_start_label, num_instances, label in groups: + for instance in range(instance_start, instance_start+num_instances): + instance_label = instance - instance_start + instance_start_label + channel_bits[instance-1] = label + ' ' + str(instance_label) + + param = { + 'description': { + 'short': 'Reverse Output Range for '+module_name, + 'long': +'''Allows to reverse the output range for each channel. +Note: this is only useful for servos. +'''.format(channel_label), + }, + 'type': 'bitmask', + 'default': 0, + 'bit': channel_bits + } + add_local_param(param_prefix+'_REV', param) + + if verbose: print('adding actuator params: {:}'.format(all_params)) + return all_params + +def load_yaml_file(file_name): + with open(file_name, 'r') as stream: + try: + return yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + raise + +output_functions_yaml = load_yaml_file(output_functions_file) +output_functions = output_functions_yaml['functions'] + +all_params = "" + +for yaml_file in args.config_files: + yaml_config = load_yaml_file(yaml_file) + + # convert 'output_groups' section into additional params + try: + actuator_output_params = get_actuator_output_params(yaml_config, + output_functions, timer_config_file, verbose) + except Exception as e: + print('Exception while parsing {:}:'.format(yaml_file)) + raise e + # now add them to the yaml config + if not 'parameters' in yaml_config: + yaml_config['parameters'] = [] + group_name = 'Actuator Outputs' + yaml_config['parameters'].append({'group': group_name, 'definitions': actuator_output_params}) + + all_params += parse_yaml_parameters_config(yaml_config, ethernet_supported) + + +if verbose: print("Generating {:}".format(params_output_file)) +with open(params_output_file, 'w') as fid: + fid.write(all_params) diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py new file mode 100755 index 000000000000..f334bd582224 --- /dev/null +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -0,0 +1,228 @@ +#!/usr/bin/env python3 +""" Script to parse board-specific timer_config.cpp and print the output groups +and timer config params to stdout +""" + +import argparse +import os +import sys +import re +from itertools import groupby +from copy import deepcopy + + +def find_matching_brackets(brackets, s, verbose): + idx = 0 + opening = 0 + first_open = -1 + while idx < len(s): + if s[idx] == brackets[0]: + opening += 1 + if first_open == -1: + first_open = idx + if s[idx] == brackets[1]: + opening -= 1 + if opening == 0: + if verbose: print(first_open, idx, s[first_open:idx+1]) + return first_open+1, idx + idx += 1 + raise Exception('Failed to find opening/closing brackets in {:}'.format(s)) + +def extract_timer(line): + # Try format: initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}), + search = re.search('Timer::([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE) + if search: + return search.group(1) + + # nxp rt1062 format: initIOPWM(PWM::FlexPWM2), + search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE) + if search: + return search.group(1) + + return None + +def extract_timer_from_channel(line): + # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE) + if search: + return search.group(1) + + # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06), + search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE) + if search: + return search.group(1) + + return None + +def get_timer_groups(timer_config_file, verbose=False): + with open(timer_config_file, 'r') as f: + timer_config = f.read() + + # timers + dshot_support = {} # key: timer + timers_start_marker = 'io_timers_t io_timers' + timers_start = timer_config.find(timers_start_marker) + if timers_start == -1: + raise Exception('"{:}" not found in {:}'.format(timers_start_marker, timer_config_file)) + timer_config = timer_config[timers_start:] + open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose) + timers_str = timer_config[open_idx:close_idx] + timers = [] + for line in timers_str.splitlines(): + line = line.strip() + if len(line) == 0 or line.startswith('//'): + continue + timer = extract_timer(line) + + if timer: + if verbose: print('found timer def: {:}'.format(timer)) + dshot_support[timer] = 'DMA' in line + timers.append(timer) + else: + # Make sure we don't miss anything (e.g. for different syntax) or misparse (e.g. multi-line comments) + raise Exception('Unparsed timer in line: {:}'.format(line)) + + + # channels + channels_start_marker = 'timer_io_channels_t timer_io_channels' + channels_start = timer_config.find(channels_start_marker) + if channels_start == -1: + raise Exception('"{:}" not found in {:}'.format(channels_start_marker, timer_config_file)) + + timer_config = timer_config[channels_start:] + open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose) + channels = timer_config[open_idx:close_idx] + channel_timers = [] + channel_types = [] + + for line in channels.splitlines(): + line = line.strip() + if len(line) == 0 or line.startswith('//'): + continue + + if verbose: print('--'+line+'--') + timer = extract_timer_from_channel(line) + + if timer: + if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line)) + channel_types.append('cap' if 'capture' in line.lower() else 'pwm') + channel_timers.append(timer) + else: + # Make sure we don't miss anything (e.g. for different syntax) or misparse (e.g. multi-line comments) + raise Exception('Unparsed channel in line: {:}'.format(line)) + + if len(channel_timers) == 0: + raise Exception('No channels found in "{:}"'.format(channels)) + + groups = [(timers.index(k), len(list(g)), dshot_support[k]) for k, g in groupby(channel_timers)] + outputs = { + 'types': channel_types, + 'groups': groups + } + + return outputs + +def get_output_groups(timer_groups, param_prefix="PWM_MAIN", + channel_labels=["PWM Main", "PWM Capture"], + standard_params=[], + extra_function_groups=[], pwm_timer_param=None, + verbose=False): + """ convert the timer groups into an output_groups section of module.yaml + and extra timer params + """ + + instance_start = 1 + output_groups = [] + timer_params = {} + instance_start_label = [ 1, 1 ] + for timer_index, group_count, dshot_support in timer_groups['groups']: + + # check for capture vs normal pins for the label + types = timer_groups['types'][instance_start-1:instance_start+group_count-1] + if not all(types[0] == t for t in types): + # Should this ever be needed, we can extend this script to handle that + raise Exception('Implementation requires all channel types for a timer to be equal (types: {:})'.format(types)) + if types[0] == 'pwm': + channel_type_idx = 0 + elif types[0] == 'cap': + channel_type_idx = 1 + else: + raise Exception('unsupported channel type: {:}'.format(types[0])) + + channel_label = channel_labels[channel_type_idx] + channel_type_instance = instance_start_label[channel_type_idx] + group_label = channel_label + ' ' + str(channel_type_instance) + if group_count > 1: + group_label += '-' + str(channel_type_instance+group_count-1) + group = { + 'param_prefix': param_prefix, + 'channel_label': channel_label, + 'instance_start': instance_start, + 'instance_start_label': channel_type_instance, + 'extra_function_groups': deepcopy(extra_function_groups), + 'num_channels': group_count, + 'standard_params': deepcopy(standard_params), + 'group_label': group_label, + 'channel_label_module_name_prefix': False, + } + + if pwm_timer_param is not None: + pwm_timer_param_cp = deepcopy(pwm_timer_param) + timer_param_name = param_prefix+'_TIM'+str(timer_index) + + group['config_parameters'] = [ + { + 'param': timer_param_name, + 'function': 'primary', + } + ] + + if dshot_support: + # don't show pwm limit params when dshot enabled + + for standard_param in group['standard_params']: + group['standard_params'][standard_param]['show_if'] = timer_param_name + '>=-1' + + # indicate support for changing motor spin direction + group['supported_actions'] = { + 'set_spin_direction1': { + 'supported_if': timer_param_name + '<-1', + 'actuator_types': ['motor'] + }, + 'set_spin_direction2': { + 'supported_if': timer_param_name + '<-1', + 'actuator_types': ['motor'] + }, + } + else: + # remove dshot entries if no dshot support + values = pwm_timer_param_cp['values'] + for key in list(values.keys()): + if 'dshot' in values[key].lower(): + del values[key] + + for descr_type in ['short', 'long']: + descr = pwm_timer_param_cp['description'][descr_type] + pwm_timer_param_cp['description'][descr_type] = \ + descr.replace('${label}', group_label) + timer_params[timer_param_name] = pwm_timer_param_cp + output_groups.append(group) + instance_start += group_count + instance_start_label[channel_type_idx] += group_count + return (output_groups, timer_params) + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Extract output groups from timer_config.cpp') + + parser.add_argument('--timer-config', type=str, action='store', + help='timer_config.cpp file', required=True) + parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', + help='Verbose Output') + + args = parser.parse_args() + verbose = args.verbose + timer_groups = get_timer_groups(args.timer_config, verbose) + print('timer groups: {:}'.format(timer_groups)) + output_groups, timer_params = get_output_groups(timer_groups, verbose=verbose) + print('output groups: {:}'.format(output_groups)) + print('timer params: {:}'.format(timer_params)) diff --git a/Tools/process_sensor_caldata.py b/Tools/process_sensor_caldata.py index 561ef292782d..625e5da4260c 100755 --- a/Tools/process_sensor_caldata.py +++ b/Tools/process_sensor_caldata.py @@ -1,4 +1,4 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 from __future__ import print_function @@ -8,6 +8,8 @@ import matplotlib.pyplot as plt import numpy as np +from scipy.signal import medfilt + from pyulog import * """ @@ -63,6 +65,9 @@ def resampleWithDeltaX(x,y): return resampledX,resampledY +def median_filter(data): + return medfilt(data, 31) + parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature') parser.add_argument('filename', metavar='file.ulg', help='ULog input file') parser.add_argument('--no_resample', dest='noResample', action='store_const', @@ -184,12 +189,16 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(gyro_0_params['TC_G0_TMIN']-gyro_0_params['TC_G0_TREF'], gyro_0_params['TC_G0_TMAX']-gyro_0_params['TC_G0_TREF'], 100) temp_resample = temp_rel_resample + gyro_0_params['TC_G0_TREF'] + sensor_gyro_0['x'] = median_filter(sensor_gyro_0['x']) + sensor_gyro_0['y'] = median_filter(sensor_gyro_0['y']) + sensor_gyro_0['z'] = median_filter(sensor_gyro_0['z']) + # fit X axis if noResample: - coef_gyro_0_x = np.polyfit(temp_rel,sensor_gyro_0['x'],3) + coef_gyro_0_x = np.polyfit(temp_rel, sensor_gyro_0['x'], 3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['x']) - coef_gyro_0_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['x']) + coef_gyro_0_x = np.polyfit(temp, sens, 3) gyro_0_params['TC_G0_X3_0'] = coef_gyro_0_x[0] gyro_0_params['TC_G0_X2_0'] = coef_gyro_0_x[1] @@ -200,10 +209,10 @@ def is_valid_directory(parser, arg): # fit Y axis if noResample: - coef_gyro_0_y = np.polyfit(temp_rel,sensor_gyro_0['y'],3) + coef_gyro_0_y = np.polyfit(temp_rel, sensor_gyro_0['y'], 3) else: temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['y']) - coef_gyro_0_y = np.polyfit(temp, sens ,3) + coef_gyro_0_y = np.polyfit(temp, sens, 3) gyro_0_params['TC_G0_X3_1'] = coef_gyro_0_y[0] gyro_0_params['TC_G0_X2_1'] = coef_gyro_0_y[1] @@ -214,9 +223,9 @@ def is_valid_directory(parser, arg): # fit Z axis if noResample: - coef_gyro_0_z = np.polyfit(temp_rel,sensor_gyro_0['z'],3) + coef_gyro_0_z = np.polyfit(temp_rel, sensor_gyro_0['z'],3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_0['z']) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_0['z']) coef_gyro_0_z = np.polyfit(temp, sens ,3) gyro_0_params['TC_G0_X3_2'] = coef_gyro_0_z[0] @@ -292,6 +301,10 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(gyro_1_params['TC_G1_TMIN']-gyro_1_params['TC_G1_TREF'], gyro_1_params['TC_G1_TMAX']-gyro_1_params['TC_G1_TREF'], 100) temp_resample = temp_rel_resample + gyro_1_params['TC_G1_TREF'] + sensor_gyro_1['x'] = median_filter(sensor_gyro_1['x']) + sensor_gyro_1['y'] = median_filter(sensor_gyro_1['y']) + sensor_gyro_1['z'] = median_filter(sensor_gyro_1['z']) + # fit X axis if noResample: coef_gyro_1_x = np.polyfit(temp_rel,sensor_gyro_1['x'],3) @@ -400,6 +413,10 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(gyro_2_params['TC_G2_TMIN']-gyro_2_params['TC_G2_TREF'], gyro_2_params['TC_G2_TMAX']-gyro_2_params['TC_G2_TREF'], 100) temp_resample = temp_rel_resample + gyro_2_params['TC_G2_TREF'] + sensor_gyro_2['x'] = median_filter(sensor_gyro_2['x']) + sensor_gyro_2['y'] = median_filter(sensor_gyro_2['y']) + sensor_gyro_2['z'] = median_filter(sensor_gyro_2['z']) + # fit X axis if noResample: coef_gyro_2_x = np.polyfit(temp_rel,sensor_gyro_2['x'],3) @@ -416,10 +433,10 @@ def is_valid_directory(parser, arg): # fit Y axis if noResample: - coef_gyro_2_y = np.polyfit(temp_rel,sensor_gyro_2['y'],3) + coef_gyro_2_y = np.polyfit(temp_rel, sensor_gyro_2['y'], 3) else: - temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['y']) - coef_gyro_2_y = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, sensor_gyro_2['y']) + coef_gyro_2_y = np.polyfit(temp, sens, 3) gyro_2_params['TC_G2_X3_1'] = coef_gyro_2_y[0] gyro_2_params['TC_G2_X2_1'] = coef_gyro_2_y[1] @@ -430,10 +447,10 @@ def is_valid_directory(parser, arg): # fit Z axis if noResample: - coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'],3) + coef_gyro_2_z = np.polyfit(temp_rel,sensor_gyro_2['z'], 3) else: temp, sens = resampleWithDeltaX(temp_rel,sensor_gyro_2['z']) - coef_gyro_2_z = np.polyfit(temp, sens ,3) + coef_gyro_2_z = np.polyfit(temp, sens, 3) gyro_2_params['TC_G2_X3_2'] = coef_gyro_2_z[0] gyro_2_params['TC_G2_X2_2'] = coef_gyro_2_z[1] @@ -508,8 +525,12 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(gyro_3_params['TC_G3_TMIN']-gyro_3_params['TC_G3_TREF'], gyro_3_params['TC_G3_TMAX']-gyro_3_params['TC_G3_TREF'], 100) temp_resample = temp_rel_resample + gyro_3_params['TC_G3_TREF'] + sensor_gyro_3['x'] = median_filter(sensor_gyro_3['x']) + sensor_gyro_3['y'] = median_filter(sensor_gyro_3['y']) + sensor_gyro_3['z'] = median_filter(sensor_gyro_3['z']) + # fit X axis - coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'],3) + coef_gyro_3_x = np.polyfit(temp_rel,sensor_gyro_3['x'], 3) gyro_3_params['TC_G3_X3_0'] = coef_gyro_3_x[0] gyro_3_params['TC_G3_X2_0'] = coef_gyro_3_x[1] gyro_3_params['TC_G3_X1_0'] = coef_gyro_3_x[2] @@ -518,7 +539,7 @@ def is_valid_directory(parser, arg): gyro_3_x_resample = fit_coef_gyro_3_x(temp_rel_resample) # fit Y axis - coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'],3) + coef_gyro_3_y = np.polyfit(temp_rel,sensor_gyro_3['y'], 3) gyro_3_params['TC_G3_X3_1'] = coef_gyro_3_y[0] gyro_3_params['TC_G3_X2_1'] = coef_gyro_3_y[1] gyro_3_params['TC_G3_X1_1'] = coef_gyro_3_y[2] @@ -527,7 +548,7 @@ def is_valid_directory(parser, arg): gyro_3_y_resample = fit_coef_gyro_3_y(temp_rel_resample) # fit Z axis - coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'],3) + coef_gyro_3_z = np.polyfit(temp_rel,sensor_gyro_3['z'], 3) gyro_3_params['TC_G3_X3_2'] = coef_gyro_3_z[0] gyro_3_params['TC_G3_X2_2'] = coef_gyro_3_z[1] gyro_3_params['TC_G3_X1_2'] = coef_gyro_3_z[2] @@ -540,8 +561,8 @@ def is_valid_directory(parser, arg): # draw plots plt.subplot(3,1,1) - plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'],'b') - plt.plot(temp_resample,gyro_3_x_resample,'r') + plt.plot(sensor_gyro_3['temperature'],sensor_gyro_3['x'], 'b') + plt.plot(temp_resample,gyro_3_x_resample, 'r') plt.title('Gyro 2 ({}) Bias vs Temperature'.format(gyro_3_params['TC_G3_ID'])) plt.ylabel('X bias (rad/s)') plt.xlabel('temperature (degC)') @@ -601,13 +622,17 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(accel_0_params['TC_A0_TMIN']-accel_0_params['TC_A0_TREF'], accel_0_params['TC_A0_TMAX']-accel_0_params['TC_A0_TREF'], 100) temp_resample = temp_rel_resample + accel_0_params['TC_A0_TREF'] + sensor_accel_0['x'] = median_filter(sensor_accel_0['x']) + sensor_accel_0['y'] = median_filter(sensor_accel_0['y']) + sensor_accel_0['z'] = median_filter(sensor_accel_0['z']) + # fit X axis correction_x = sensor_accel_0['x'] - np.median(sensor_accel_0['x']) if noResample: - coef_accel_0_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_0_x = np.polyfit(temp_rel,correction_x, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_0_x = np.polyfit(temp, sens ,3) + coef_accel_0_x = np.polyfit(temp, sens, 3) accel_0_params['TC_A0_X3_0'] = coef_accel_0_x[0] accel_0_params['TC_A0_X2_0'] = coef_accel_0_x[1] @@ -617,12 +642,12 @@ def is_valid_directory(parser, arg): correction_x_resample = fit_coef_accel_0_x(temp_rel_resample) # fit Y axis - correction_y = sensor_accel_0['y']-np.median(sensor_accel_0['y']) + correction_y = sensor_accel_0['y'] - np.median(sensor_accel_0['y']) if noResample: - coef_accel_0_y = np.polyfit(temp_rel,correction_y,3) + coef_accel_0_y = np.polyfit(temp_rel, correction_y, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_y) - coef_accel_0_y = np.polyfit(temp, sens ,3) + coef_accel_0_y = np.polyfit(temp, sens, 3) accel_0_params['TC_A0_X3_1'] = coef_accel_0_y[0] accel_0_params['TC_A0_X2_1'] = coef_accel_0_y[1] @@ -632,12 +657,12 @@ def is_valid_directory(parser, arg): correction_y_resample = fit_coef_accel_0_y(temp_rel_resample) # fit Z axis - correction_z = sensor_accel_0['z']-np.median(sensor_accel_0['z']) + correction_z = sensor_accel_0['z'] - np.median(sensor_accel_0['z']) if noResample: - coef_accel_0_z = np.polyfit(temp_rel,correction_z,3) + coef_accel_0_z = np.polyfit(temp_rel,correction_z, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_accel_0_z = np.polyfit(temp, sens ,3) + coef_accel_0_z = np.polyfit(temp, sens, 3) accel_0_params['TC_A0_X3_2'] = coef_accel_0_z[0] accel_0_params['TC_A0_X2_2'] = coef_accel_0_z[1] @@ -712,13 +737,17 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(accel_1_params['TC_A1_TMIN']-accel_1_params['TC_A1_TREF'], accel_1_params['TC_A1_TMAX']-accel_1_params['TC_A1_TREF'], 100) temp_resample = temp_rel_resample + accel_1_params['TC_A1_TREF'] + sensor_accel_1['x'] = median_filter(sensor_accel_1['x']) + sensor_accel_1['y'] = median_filter(sensor_accel_1['y']) + sensor_accel_1['z'] = median_filter(sensor_accel_1['z']) + # fit X axis - correction_x = sensor_accel_1['x']-np.median(sensor_accel_1['x']) + correction_x = sensor_accel_1['x'] - np.median(sensor_accel_1['x']) if noResample: - coef_accel_1_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_1_x = np.polyfit(temp_rel, correction_x, 3) else: - temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_1_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_accel_1_x = np.polyfit(temp, sens, 3) accel_1_params['TC_A1_X3_0'] = coef_accel_1_x[0] accel_1_params['TC_A1_X2_0'] = coef_accel_1_x[1] @@ -728,7 +757,7 @@ def is_valid_directory(parser, arg): correction_x_resample = fit_coef_accel_1_x(temp_rel_resample) # fit Y axis - correction_y = sensor_accel_1['y']-np.median(sensor_accel_1['y']) + correction_y = sensor_accel_1['y'] - np.median(sensor_accel_1['y']) if noResample: coef_accel_1_y = np.polyfit(temp_rel,correction_y,3) else: @@ -743,12 +772,12 @@ def is_valid_directory(parser, arg): correction_y_resample = fit_coef_accel_1_y(temp_rel_resample) # fit Z axis - correction_z = (sensor_accel_1['z'])-np.median(sensor_accel_1['z']) + correction_z = sensor_accel_1['z'] - np.median(sensor_accel_1['z']) if noResample: - coef_accel_1_z = np.polyfit(temp_rel,correction_z,3) + coef_accel_1_z = np.polyfit(temp_rel,correction_z, 3) else: temp, sens = resampleWithDeltaX(temp_rel,correction_z) - coef_accel_1_z = np.polyfit(temp, sens ,3) + coef_accel_1_z = np.polyfit(temp, sens, 3) accel_1_params['TC_A1_X3_2'] = coef_accel_1_z[0] accel_1_params['TC_A1_X2_2'] = coef_accel_1_z[1] @@ -824,13 +853,17 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(accel_2_params['TC_A2_TMIN']-accel_2_params['TC_A2_TREF'], accel_2_params['TC_A2_TMAX']-accel_2_params['TC_A2_TREF'], 100) temp_resample = temp_rel_resample + accel_2_params['TC_A2_TREF'] + sensor_accel_2['x'] = median_filter(sensor_accel_2['x']) + sensor_accel_2['y'] = median_filter(sensor_accel_2['y']) + sensor_accel_2['z'] = median_filter(sensor_accel_2['z']) + # fit X axis - correction_x = sensor_accel_2['x']-np.median(sensor_accel_2['x']) + correction_x = sensor_accel_2['x'] - np.median(sensor_accel_2['x']) if noResample: - coef_accel_2_x = np.polyfit(temp_rel,correction_x,3) + coef_accel_2_x = np.polyfit(temp_rel,correction_x, 3) else: - temp, sens = resampleWithDeltaX(temp_rel,correction_x) - coef_accel_2_x = np.polyfit(temp, sens ,3) + temp, sens = resampleWithDeltaX(temp_rel, correction_x) + coef_accel_2_x = np.polyfit(temp, sens, 3) accel_2_params['TC_A2_X3_0'] = coef_accel_2_x[0] accel_2_params['TC_A2_X2_0'] = coef_accel_2_x[1] @@ -840,7 +873,7 @@ def is_valid_directory(parser, arg): correction_x_resample = fit_coef_accel_2_x(temp_rel_resample) # fit Y axis - correction_y = sensor_accel_2['y']-np.median(sensor_accel_2['y']) + correction_y = sensor_accel_2['y'] - np.median(sensor_accel_2['y']) if noResample: coef_accel_2_y = np.polyfit(temp_rel,correction_y,3) else: @@ -855,7 +888,7 @@ def is_valid_directory(parser, arg): correction_y_resample = fit_coef_accel_2_y(temp_rel_resample) # fit Z axis - correction_z = sensor_accel_2['z']-np.median(sensor_accel_2['z']) + correction_z = sensor_accel_2['z'] - np.median(sensor_accel_2['z']) if noResample: coef_accel_2_z = np.polyfit(temp_rel,correction_z,3) else: @@ -935,9 +968,13 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(accel_3_params['TC_A3_TMIN']-accel_3_params['TC_A3_TREF'], accel_3_params['TC_A3_TMAX']-accel_3_params['TC_A3_TREF'], 100) temp_resample = temp_rel_resample + accel_3_params['TC_A3_TREF'] + sensor_accel_3['x'] = median_filter(sensor_accel_3['x']) + sensor_accel_3['y'] = median_filter(sensor_accel_3['y']) + sensor_accel_3['z'] = median_filter(sensor_accel_3['z']) + # fit X axis - correction_x = sensor_accel_3['x']-np.median(sensor_accel_3['x']) - coef_accel_3_x = np.polyfit(temp_rel,correction_x,3) + correction_x = sensor_accel_3['x'] - np.median(sensor_accel_3['x']) + coef_accel_3_x = np.polyfit(temp_rel, correction_x, 3) accel_3_params['TC_A3_X3_0'] = coef_accel_3_x[0] accel_3_params['TC_A3_X2_0'] = coef_accel_3_x[1] accel_3_params['TC_A3_X1_0'] = coef_accel_3_x[2] @@ -946,8 +983,8 @@ def is_valid_directory(parser, arg): correction_x_resample = fit_coef_accel_3_x(temp_rel_resample) # fit Y axis - correction_y = sensor_accel_3['y']-np.median(sensor_accel_3['y']) - coef_accel_3_y = np.polyfit(temp_rel,correction_y,3) + correction_y = sensor_accel_3['y'] - np.median(sensor_accel_3['y']) + coef_accel_3_y = np.polyfit(temp_rel, correction_y, 3) accel_3_params['TC_A3_X3_1'] = coef_accel_3_y[0] accel_3_params['TC_A3_X2_1'] = coef_accel_3_y[1] accel_3_params['TC_A3_X1_1'] = coef_accel_3_y[2] @@ -956,8 +993,8 @@ def is_valid_directory(parser, arg): correction_y_resample = fit_coef_accel_3_y(temp_rel_resample) # fit Z axis - correction_z = sensor_accel_3['z']-np.median(sensor_accel_3['z']) - coef_accel_3_z = np.polyfit(temp_rel,correction_z,3) + correction_z = sensor_accel_3['z'] - np.median(sensor_accel_3['z']) + coef_accel_3_z = np.polyfit(temp_rel, correction_z, 3) accel_3_params['TC_A3_X3_2'] = coef_accel_3_z[0] accel_3_params['TC_A3_X2_2'] = coef_accel_3_z[1] accel_3_params['TC_A3_X1_2'] = coef_accel_3_z[2] @@ -1024,8 +1061,10 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(baro_0_params['TC_B0_TMIN']-baro_0_params['TC_B0_TREF'], baro_0_params['TC_B0_TMAX']-baro_0_params['TC_B0_TREF'], 100) temp_resample = temp_rel_resample + baro_0_params['TC_B0_TREF'] +sensor_baro_0['pressure'] = median_filter(sensor_baro_0['pressure']) + # fit data -median_pressure = np.median(sensor_baro_0['pressure']); +median_pressure = np.median(sensor_baro_0['pressure']) if noResample: coef_baro_0_x = np.polyfit(temp_rel,100*(sensor_baro_0['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1081,8 +1120,10 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(baro_1_params['TC_B1_TMIN']-baro_1_params['TC_B1_TREF'], baro_1_params['TC_B1_TMAX']-baro_1_params['TC_B1_TREF'], 100) temp_resample = temp_rel_resample + baro_1_params['TC_B1_TREF'] + sensor_baro_1['pressure'] = median_filter(sensor_baro_1['pressure']) + # fit data - median_pressure = np.median(sensor_baro_1['pressure']); + median_pressure = np.median(sensor_baro_1['pressure']) if noResample: coef_baro_1_x = np.polyfit(temp_rel,100*(sensor_baro_1['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1139,8 +1180,10 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(baro_2_params['TC_B2_TMIN']-baro_2_params['TC_B2_TREF'], baro_2_params['TC_B2_TMAX']-baro_2_params['TC_B2_TREF'], 100) temp_resample = temp_rel_resample + baro_2_params['TC_B2_TREF'] + sensor_baro_2['pressure'] = median_filter(sensor_baro_2['pressure']) + # fit data - median_pressure = np.median(sensor_baro_2['pressure']); + median_pressure = np.median(sensor_baro_2['pressure']) if noResample: coef_baro_2_x = np.polyfit(temp_rel,100*(sensor_baro_2['pressure']-median_pressure),5) # convert from hPa to Pa else: @@ -1197,6 +1240,8 @@ def is_valid_directory(parser, arg): temp_rel_resample = np.linspace(baro_3_params['TC_B3_TMIN']-baro_3_params['TC_B3_TREF'], baro_3_params['TC_B3_TMAX']-baro_3_params['TC_B3_TREF'], 100) temp_resample = temp_rel_resample + baro_3_params['TC_B3_TREF'] + sensor_baro_3['pressure'] = median_filter(sensor_baro_3['pressure']) + # fit data median_pressure = np.median(sensor_baro_3['pressure']) coef_baro_3_x = np.polyfit(temp_rel,100*(sensor_baro_3['pressure']-median_pressure),5) # convert from hPa to Pa diff --git a/Tools/px4airframes/markdownout.py b/Tools/px4airframes/markdownout.py index 92987cd75953..5386404f717e 100644 --- a/Tools/px4airframes/markdownout.py +++ b/Tools/px4airframes/markdownout.py @@ -5,17 +5,52 @@ class MarkdownTablesOutput(): def __init__(self, groups, board, image_path): - result = ("# Airframes Reference\n" - "> **Note** **This list is [auto-generated](https://github.com/PX4/Firmware/edit/master/Tools/px4airframes/markdownout.py) from the source code**.\n" - "> \n" - "> **AUX** channels may not be present on some flight controllers.\n" - "> If present, PWM AUX channels are commonly labelled **AUX OUT**.\n" - "> \n" - "\n") - - result += """This page lists all supported airframes and types including - the motor assignment and numbering. The motors in **green** rotate clockwise, - the ones in **blue** counterclockwise.\n\n""" + result = """# Airframes Reference + +:::note +**This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`. +::: + +This page lists all supported airframes and types including the motor assignment and numbering. +The motors in **green** rotate clockwise, the ones in **blue** counterclockwise. + +**AUX** channels may not be present on some flight controllers. +If present, PWM AUX channels are commonly labelled **AUX OUT**. + +\n\n""" type_set = set() @@ -31,51 +66,39 @@ def __init__(self, groups, board, image_path): # Display an image of the frame image_name = group.GetImageName() - result += '
\n' + result += '
\n' image_name = image_path + image_name - result += '\n' % (image_name) + result += '\n' % (image_name) # check if all outputs are equal for the group: if so, show them # only once - outputs_prev = ['', ''] # split into MAINx and others (AUXx) - outputs_match = [True, True] + all_outputs = {} + num_configs = len(group.GetParams()) for param in group.GetParams(): if not self.IsExcluded(param, board): - outputs_current = ['', ''] for output_name in param.GetOutputCodes(): value = param.GetOutputValue(output_name) - if output_name.lower().startswith('main'): - idx = 0 - else: - idx = 1 - outputs_current[idx] += '
  • %s: %s
  • ' % (output_name, value) - for i in range(2): - if len(outputs_current[i]) != 0: - if outputs_prev[i] == '': - outputs_prev[i] = outputs_current[i] - elif outputs_current[i] != outputs_prev[i]: - outputs_match[i] = False - - for i in range(2): - if len(outputs_prev[i]) == 0: - outputs_match[i] = False - if not outputs_match[i]: - outputs_prev[i] = '' - - if outputs_match[0] or outputs_match[1]: - result += '\n' - result += ' \n' + key_value_pair = (output_name, value) + if key_value_pair not in all_outputs: + all_outputs[key_value_pair] = 0 + all_outputs[key_value_pair] += 1 + has_common_outputs = any(all_outputs[k] == num_configs for k in all_outputs) + + if has_common_outputs: + outputs_common = ''.join(['
  • {:}: {:}
  • '.format(k[0], k[1]) \ + for k in all_outputs if all_outputs[k] == num_configs]) + result += '
    \n' result += ' \n' result += ' \n' result += ' \n' - result += '\n' - result += '\n \n\n' % (outputs_prev[0], outputs_prev[1]) + result += ' \n' + result += '\n \n\n' % (outputs_common) result += '
    Common Outputs
      %s%s
      %s
    \n' result += '
    \n\n' - result += '\n' - result += ' \n' + result += '
    \n' + result += '
    \n' result += ' \n' result += ' \n' result += ' \n' @@ -90,10 +113,12 @@ def __init__(self, groups, board, image_path): maintainer = param.GetMaintainer() maintainer_entry = '' if maintainer != '': - maintainer_entry = '

    Maintainer: %s

    ' % (html.escape(maintainer)) + maintainer_entry = 'Maintainer: %s' % (html.escape(maintainer)) url = param.GetFieldValue('url') - name_anchor='id="%s_%s_%s"' % (group.GetClass(),group.GetName(),name) + name_anchor='%s_%s_%s' % (group.GetClass(),group.GetName(),name) name_anchor=name_anchor.replace(' ','_').lower() + name_anchor=name_anchor.replace('"','_').lower() + name_anchor='id="%s"' % name_anchor name_entry = name if url != '': name_entry = '%s' % (url, name) @@ -102,11 +127,8 @@ def __init__(self, groups, board, image_path): for output_name in param.GetOutputCodes(): value = param.GetOutputValue(output_name) valstrs = value.split(";") - if output_name.lower().startswith('main'): - idx = 0 - else: - idx = 1 - if not outputs_match[idx]: + key_value_pair = (output_name, value) + if all_outputs[key_value_pair] < num_configs: outputs += '
  • %s: %s
  • ' % (output_name, value) has_outputs = True @@ -120,13 +142,13 @@ def __init__(self, groups, board, image_path): else: outputs_entry = '' - result += ('\n \n \n\n\n' % + result += ('\n \n \n\n' % (name_anchor, name_entry, maintainer_entry, airframe_id_entry, outputs_entry)) #Close the table. - result += '
    Name
    %s%s%s%s
    %s%s%s%s
    \n\n' + result += '\n\n
    \n\n' self.output = result diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py index 330c0793aba8..302cbf012d59 100644 --- a/Tools/px4airframes/rcout.py +++ b/Tools/px4airframes/rcout.py @@ -74,6 +74,7 @@ def __init__(self, groups, board, post_start=False): result += "\n" result += "if [ ${AIRFRAME} != none ]\n" result += "then\n" + result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n" result += "\t. /etc/init.d/airframes/${AIRFRAME}\n" if not post_start: result += "else\n" diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index 6a8a8bcd4ad6..2fd314d67f02 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -98,6 +98,8 @@ def GetImageName(self): return "Boat" elif (self.name == "Balloon"): return "Balloon" + elif (self.name == "Vectored 6 DOF UUV"): + return "Vectored6DofUUV" return "AirframeUnknown" def GetParams(self): diff --git a/Tools/px4events/jsonout.py b/Tools/px4events/jsonout.py new file mode 100644 index 000000000000..7e5affb08aed --- /dev/null +++ b/Tools/px4events/jsonout.py @@ -0,0 +1,57 @@ +import codecs +import json +import sys +import os + + +class JsonOutput(): + def __init__(self, groups): + all_json = {} + all_json['version'] = 1 + component = {} + all_json['components'] = {1: component} #1: autopilot component + + all_events = {} + component['namespace'] = "px4" + component['event_groups'] = all_events + + for group in groups: + current_group = {} + current_events = {} + current_group['events'] = current_events + all_events[group] = current_group + + for e in groups[group]: + event_obj = {} + event_obj['name'] = e.name + event_obj['message'] = e.message + if e.description is not None: + event_obj['description'] = e.description + args = [] + for i in range(len(e.arguments)): + arg = {} + arg['type'] = e.arguments[i][0] + arg['name'] = e.arguments[i][1] + args.append(arg) + if len(args) > 0: + event_obj['arguments'] = args + sub_id = e.sub_id + assert sub_id not in current_events, \ + "Duplicate event ID for {0} (message: '{1}'), other event message: '{2}'".format( + e.name, e.message, current_events[sub_id]['message']) + current_events[sub_id] = event_obj + + self.json = all_json + + def save(self, filename): + need_to_write = True + # only write if current file is not the same, to avoid updating the file + # timestamp + if os.path.isfile(filename): + with open(filename, 'rb') as json_file: + existing_data = json.load(json_file) + if existing_data == self.json: + need_to_write = False + if need_to_write: + with codecs.open(filename, 'w', 'utf-8') as f: + f.write(json.dumps(self.json,indent=2)) diff --git a/Tools/px4events/srcparser.py b/Tools/px4events/srcparser.py new file mode 100644 index 000000000000..7a6c643ccaba --- /dev/null +++ b/Tools/px4events/srcparser.py @@ -0,0 +1,309 @@ +import sys +import re +import math + +def hash_32_fnv1a(data: str): + hash_val = 0x811c9dc5 + prime = 0x1000193 + for i in range(len(data)): + value = ord(data[i]) + hash_val = hash_val ^ value + hash_val *= prime + hash_val &= 0xffffffff + return hash_val + + +class Event(object): + """ + Single event definition + """ + + def __init__(self): + self.name = None + self.message = None + self.description = None + self.group = "default" + self._arguments = [] + + @staticmethod + def _get_id(name): + return 0xffffff & hash_32_fnv1a(name) + + @property + def arguments(self): + """ list of (type: str, name: str) tuples """ + return self._arguments + + def set_default_arguments(self, num_args): + """ set argument names to default (if not specified) """ + for i in range(num_args): + self.add_argument(None, "arg"+str(i)) + + def _shift_printed_arguments(self, msg, offset): + """ shift all { arguments by an offset """ + i = 0 + while i < len(msg): + + if msg[i] == '\\': # escaped character + i += 2 + continue + + if msg[i] == '{': + m = re.match(r"^(\d+)", msg[i+1:]) + if m: + arg_idx = int(m.group(1)) + offset + msg = msg[:i+1] + str(arg_idx) + msg[i+1+len(m.group(1)):] + i += 1 + return msg + + def prepend_arguments(self, arguments): + """ prepend additional arguments, and shift all '{}' in the + description and message + :param arguments: list of (type: str, name: str) tuples + """ + self._arguments = arguments + self._arguments + num_added = len(arguments) + if self.message is not None: + self.message = self._shift_printed_arguments(self.message, num_added) + if self.description is not None: + self.description = self._shift_printed_arguments(self.description, num_added) + + def add_argument(self, arg_type, name): + self._arguments.append((arg_type, name)) + + @property + def sub_id(self): + return self._get_id(self.name) + + def validate(self): + if self.name is None: raise Exception("missing event name") + if self.message is None: raise Exception("missing event message for {}".format(self.name)) + # just to ensure a common convention + assert self.message[-1] != '.', "Avoid event message ending in '.' ({:})".format(self.message) + # description is optional + +class SourceParser(object): + """ + Parses provided data and stores all found events internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\s*EVENT$') + re_events_send = re.compile(r'^events::send[<\(]') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'^@([a-zA-Z][a-zA-Z0-9_]*):?\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/$') + re_code_end = re.compile(r'(.*?)\s*;$') + re_template_args = re.compile(r'([a-zA-Z0-9_:\.]+)\s*<([a-zA-Z0-9_,\s:]+)\s*>\s*\((.*)\);$') + re_no_template_args = re.compile(r'([a-zA-Z0-9_:\.]+)\s*\((.*)\);$') + re_event_id = re.compile(r'(events::)?ID\("([a-zA-Z0-9_]+)\"') + + def __init__(self): + self._events = {} + + @property + def events(self): + """ dict of 'group': [Event] list """ + return self._events + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found events + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + def finalize_current_tag(event, tag, value): + if tag is None: return + if tag == "description": + descr = value.strip() + # merge continued lines (but not e.g. enumerations) + for i in range(1, len(descr)-1): + if descr[i-1] != '\n' and descr[i] == '\n' and descr[i+1].isalpha(): + descr = descr[:i] + ' ' + descr[i+1:] + event.description = descr + elif tag == "group": + known_groups = ["calibration", "health", "arming_check", "normal"] + event.group = value.strip() + if not event.group in known_groups: + raise Exception("Unknown event group: '{}'\nKnown groups: {}\n" \ + "If this is not a typo, add the new group to the script".format(event.group, known_groups)) + elif tag.startswith("arg"): + arg_index = int(tag[3:])-1 + arg_name = value.strip() + assert len(event.arguments) == arg_index, "Invalid argument ordering/duplicate ({}, {})".format(tag, value) + event.add_argument(None, arg_name) + else: + raise Exception("Invalid tag: {}\nvalue: {}".format(tag, value)) + + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + + assert not line.startswith("using namespace events;"), "Avoid 'using namespace events;', as it prevents proper events extraction" + + # Check for '/* EVENT' + if self.re_comment_start.match(line): + state = "parse-comments" + event = Event() + current_tag = None + current_value = None + current_code = "" + continue + + # Check for events::send (allow '/* EVENT' to be optional) + if state is None and self.re_events_send.match(line): + state = "parse-command" + event = Event() + current_tag = None + current_value = None + current_code = "" + + if state is None: + assert 'events::ID(' not in line or line.startswith('//'), \ + "unmatched 'events::ID(' found in line '{:}'".format(line) + continue + + if state == "parse-command": + current_code += line + m = self.re_code_end.search(line) + if m: + # extract template arguments + m = self.re_template_args.search(current_code) + if m: + call, template_args, args = m.group(1, 2, 3) + template_args = template_args.split(',') + else: + m = self.re_no_template_args.search(current_code) + if m: + template_args = [] + call, args = m.group(1, 2) + else: + raise Exception("Failed to parse code line {:}".format(current_code)) + + # if event arguments are not specified, use default naming + if len(event.arguments) == 0: + event.set_default_arguments(len(template_args)) + + # get argument types from template arguments + assert len(template_args) == len(event.arguments), \ + "Number of arguments mismatch (args: {:})".format(template_args) + num_args = len(template_args) + for i in range(num_args): + arg_name = event.arguments[i][1] + arg_type = template_args[i].strip() + if arg_type.startswith('events::'): + arg_type = arg_type[8:] + arg_type = arg_type.replace('enums::', '') + event.arguments[i] = (arg_type, arg_name) + #print("method: {}, args: {}, template args: {}".format(call, args, event.arguments)) + + ignore_event = False + + # extract function arguments + args_split = self._parse_arguments(args) + if call == "events::send" or call == "send": + if len(args_split) == 1: + # This is a send call for a generated event + ignore_event = True + else: + assert len(args_split) == num_args + 3, \ + "Unexpected Number of arguments for: {:}, " \ + "num template args: {:} (missing template args?)" \ + .format(args_split, num_args) + m = self.re_event_id.search(args_split[0]) + if m: + _, event_name = m.group(1, 2) + else: + raise Exception("Could not extract event ID from {:}".format(args_split[0])) + event.name = event_name + # unescape \x, to treat the string the same as the C++ compiler + event.message = args_split[2][1:-1].encode("utf-8").decode('unicode_escape') + elif call in ['reporter.healthFailure', 'reporter.armingCheckFailure']: + assert len(args_split) == num_args + 5, \ + "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args) + m = self.re_event_id.search(args_split[2]) + if m: + _, event_name = m.group(1, 2) + else: + raise Exception("Could not extract event ID from {:}".format(args_split[2])) + event.name = event_name + event.message = args_split[4][1:-1] + if 'health' in call: + event.group = "health" + else: + event.group = "arming_check" + event.prepend_arguments([('common::navigation_mode_category_t', 'modes'), + ('uint8_t', 'health_component_index')]) + else: + raise Exception("unknown event method call: {}, args: {}".format(call, args)) + + if not ignore_event: + event.validate() + + # insert + if not event.group in self._events: + self._events[event.group] = [] + self._events[event.group].append(event) + + state = None + + else: # parse-comments + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + m = self.re_comment_tag.match(comment_content) + if m: + finalize_current_tag(event, current_tag, current_value) + current_tag, current_value = m.group(1, 2) + elif current_tag is not None: + current_value += "\n"+comment_content + # else: empty line before any tag + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". + raise Exception("Excpected a comment, got '{}'".format(line)) + if last_comment_line: + finalize_current_tag(event, current_tag, current_value) + state = "parse-command" + return True + + def _parse_arguments(self, args): + """ + given a string of arguments, returns a list of strings split into the + arguments, with respecting brackets. + args is expected to be a single line. + Note: comments are not handled, also template arguments. + + e.g. "32, test(4,4), \"e(c\", ab" -> ["32", "test(4,4)", "\"e(c\"", "ab"] + """ + args_split = [] + start = 0 + bracket = 0 + in_string = False + for i in range(len(args)): + if in_string and args[i] == "\"" and args[i-1] != "\\": + in_string = False + elif not in_string and args[i] == "\"": + in_string = True + if in_string: + continue + if args[i] in "{([": + bracket += 1 + if args[i] in "})]": + bracket -= 1 + if bracket == 0 and args[i] == ',': + args_split.append(args[start:i].strip()) + start = i + 1 + args_split.append(args[start:].strip()) + return args_split diff --git a/Tools/px4events/srcscanner.py b/Tools/px4events/srcscanner.py new file mode 100644 index 000000000000..3f14fb949d75 --- /dev/null +++ b/Tools/px4events/srcscanner.py @@ -0,0 +1,52 @@ +import os +import re +import codecs +import sys + +class SourceScanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + def ScanDir(self, srcdirs, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = tuple([".cpp"]) + for srcdir in srcdirs: + if os.path.isfile(srcdir): + if not self.ScanFile(srcdir, parser): + return False + else: + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + if filename.endswith(extensions): + path = os.path.join(dirname, filename) + try: + if not self.ScanFile(path, parser): + return False + except: + print(("Exception in file %s" % path)) + raise + return True + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + + with codecs.open(path, 'r', 'utf-8') as f: + try: + contents = f.read() + except: + contents = '' + print('Failed reading file: %s, skipping content.' % path) + pass + try: + return parser.Parse(contents) + except Exception as e: + print("Exception while parsing file {}".format(path)) + raise diff --git a/Tools/px4moduledoc/markdownout.py b/Tools/px4moduledoc/markdownout.py index 2b8dbbba3edf..9896c091b4ec 100644 --- a/Tools/px4moduledoc/markdownout.py +++ b/Tools/px4moduledoc/markdownout.py @@ -28,7 +28,7 @@ def __init__(self, module_groups): Since this is generated from source, errors must be reported/fixed in the [PX4-Autopilot](https://github.com/PX4/PX4-Autopilot) repository. The documentation pages can be generated by running the following command from -the root of the Firmware directory: +the root of the PX4-Autopilot directory: ``` make module_documentation ``` @@ -66,7 +66,7 @@ def _ProcessModules(self, module_list): result = '' for module in module_list: result += "## %s\n" % module.name() - result += "Source: [%s](https://github.com/PX4/Firmware/tree/master/src/%s)\n\n" % (module.scope(), module.scope()) + result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/master/src/%s)\n\n" % (module.scope(), module.scope()) doc = module.documentation() if len(doc) > 0: result += "%s\n" % doc diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index 1f42123b4c2b..b843a235f1f4 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -14,9 +14,9 @@ class ModuleDocumentation(object): # If you add categories or subcategories, they also need to be added to the # TOC in https://github.com/PX4/Devguide/blob/master/en/SUMMARY.md valid_categories = ['driver', 'estimator', 'controller', 'system', - 'communication', 'command', 'template', 'simulation'] + 'communication', 'command', 'template', 'simulation', 'autotune'] valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor', - 'magnetometer', 'baro', 'optical_flow'] + 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder'] max_line_length = 80 # wrap lines that are longer than this @@ -137,8 +137,8 @@ def _handle_usage_params_i2c_spi_driver(self, args): "\"board-specific bus (default=all) (external SPI: n-th bus (default=1))\"", 'true']) if self._is_bool_true(args[1]): - self._handle_usage_param_int(['\'c\'', '1', '1', '10', - "\"chip-select index (for external SPI)\"", 'true']) + self._handle_usage_param_int(['\'c\'', '-1', '0', '31', + "\"chip-select pin (for internal SPI) or index (for external SPI)\"", 'true']) self._handle_usage_param_int(['\'m\'', '-1', '0', '3', "\"SPI mode\"", 'true']) self._handle_usage_param_int(['\'f\'', '-1', '0', '1000000', "\"bus frequency in kHz\"", 'true']) diff --git a/Tools/px_process_events.py b/Tools/px_process_events.py new file mode 100755 index 000000000000..66e9aaa52910 --- /dev/null +++ b/Tools/px_process_events.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 events processor (main executable file) +# +# This tool scans the PX4 source code for definitions of events. +# + +import sys +import os +import argparse +from px4events import srcscanner, srcparser, jsonout + +import re +import codecs + + +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser(description="Process events definitions.") + parser.add_argument("-s", "--src-path", + default=["../src"], + metavar="PATH", + nargs='*', + help="one or more paths/files to source files to scan for events") + parser.add_argument("-j", "--json", + nargs='?', + const="events.json", + metavar="FILENAME", + help="Create Json output file" + " (default FILENAME: events.json)") + parser.add_argument('-v', '--verbose', + action='store_true', + help="verbose output") + + args = parser.parse_args() + + # Check for valid command + if not (args.json): + print("Error: You need to specify at least one output method!") + parser.print_usage() + sys.exit(1) + + # Initialize source scanner and parser + scanner = srcscanner.SourceScanner() + parser = srcparser.SourceParser() + + # Scan directories, and parse the files + if args.verbose: + print("Scanning source path/files " + str(args.src_path)) + + # canonicalize + remove duplicates + src_paths = set() + for path in args.src_path: + src_paths.add(os.path.realpath(path)) + + if not scanner.ScanDir(src_paths, parser): + sys.exit(1) + + events = parser.events + + # Output to JSON file + if args.json: + if args.verbose: print("Creating Json file " + args.json) + cur_dir = os.path.dirname(os.path.realpath(__file__)) + out = jsonout.JsonOutput(events) + out.save(args.json) + +if __name__ == "__main__": + main() diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index aa25560a6404..b35e5bc372bf 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -56,7 +56,6 @@ import sys import argparse import binascii -import serial import socket import struct import json @@ -68,6 +67,16 @@ from sys import platform as _platform +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) + # Detect python version if sys.version_info[0] < 3: runningPython3 = False @@ -673,7 +682,28 @@ def __next_baud_flightstack(self): return True - def send_reboot(self): + def send_protocol_splitter_format(self, data): + # Header Structure: + # bits: 1 2 3 4 5 6 7 8 + # header[0] - | Magic | (='S') + # header[1] - |T| LenH | (T - 0: mavlink; 1: rtps) + # header[2] - | LenL | + # header[3] - | Checksum | + + MAGIC = 83 + + len_bytes = len(data).to_bytes(2, "big") + LEN_H = len_bytes[0] & 127 + LEN_L = len_bytes[1] & 255 + CHECKSUM = MAGIC ^ LEN_H ^ LEN_L + + header_ints = [MAGIC, LEN_H, LEN_L, CHECKSUM] + header_bytes = struct.pack("{}B".format(len(header_ints)), *header_ints) + + self.__send(header_bytes) + self.__send(data) + + def send_reboot(self, use_protocol_splitter_format=False): if (not self.__next_baud_flightstack()): return False @@ -684,15 +714,19 @@ def send_reboot(self): print("If the board does not respond, unplug and re-plug the USB connector.", file=sys.stderr) try: + send_fct = self.__send + if use_protocol_splitter_format: + send_fct = self.send_protocol_splitter_format + # try MAVLINK command first self.port.flush() - self.__send(uploader.MAVLINK_REBOOT_ID1) - self.__send(uploader.MAVLINK_REBOOT_ID0) + send_fct(uploader.MAVLINK_REBOOT_ID1) + send_fct(uploader.MAVLINK_REBOOT_ID0) # then try reboot via NSH - self.__send(uploader.NSH_INIT) - self.__send(uploader.NSH_REBOOT_BL) - self.__send(uploader.NSH_INIT) - self.__send(uploader.NSH_REBOOT) + send_fct(uploader.NSH_INIT) + send_fct(uploader.NSH_REBOOT_BL) + send_fct(uploader.NSH_INIT) + send_fct(uploader.NSH_REBOOT) self.port.flush() self.port.baudrate = self.baudrate_bootloader except Exception: @@ -717,38 +751,19 @@ def main(): parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.") 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('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() + if args.use_protocol_splitter_format: + print("Using protocol splitter format to reboot pixhawk!") + # warn people about ModemManager which interferes badly with Pixhawk if os.path.exists("/usr/sbin/ModemManager"): print("==========================================================================================================") print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)") print("==========================================================================================================") - # We need to check for pyserial because the import itself doesn't - # seem to fail, at least not on macOS. - pyserial_installed = False - try: - if serial.__version__: - pyserial_installed = True - except: - pass - - try: - if serial.VERSION: - pyserial_installed = True - except: - pass - - if not pyserial_installed: - print("Error: pyserial not installed!") - print("") - print("You may need to install it using:") - print(" pip3 install --user pyserial") - print("") - sys.exit(1) - # Load the firmware file fw = firmware(args.firmware) @@ -844,7 +859,7 @@ def main(): except Exception: - if not up.send_reboot(): + if not up.send_reboot(args.use_protocol_splitter_format): break # wait for the reboot, without we might run into Serial I/O Error 5 diff --git a/Tools/serial/generate_config.py b/Tools/serial/generate_config.py index 27003b904bba..84a9c342aad4 100755 --- a/Tools/serial/generate_config.py +++ b/Tools/serial/generate_config.py @@ -1,8 +1,6 @@ #!/usr/bin/env python3 """ Script to generate Serial (UART) parameters and the ROMFS startup script """ -from __future__ import print_function - import argparse import os import sys @@ -138,6 +136,12 @@ "default_baudrate": 1, # set default to an unusable value to detect that this serial port has not been configured }, + # Pixhawk Payload Bus + "PPB": { + "label": "Pixhawk Payload Bus", + "index": 401, + "default_baudrate": 57600, + }, } @@ -189,8 +193,6 @@ # parse the YAML files serial_commands = [] ethernet_configuration = [] -additional_params = "" -additional_ethernet_params = "" if ethernet_supported: ethernet_configuration.append({ @@ -211,161 +213,12 @@ def parse_yaml_serial_config(yaml_config): ret.append(serial_config) return ret -def parse_yaml_parameters_config(yaml_config): - """ parse the parameters section from the yaml config file """ - if 'parameters' not in yaml_config: - return '' - parameters_section_list = yaml_config['parameters'] - for parameters_section in parameters_section_list: - if 'definitions' not in parameters_section: - return '' - definitions = parameters_section['definitions'] - ret = '' - param_group = parameters_section.get('group', None) - for param_name in definitions: - param = definitions[param_name] - if 'ethernet' in param: - continue - num_instances = param.get('num_instances', 1) - instance_start = param.get('instance_start', 0) # offset - - # get the type and extract all tags - tags = '@group {:}'.format(param_group) - if param['type'] == 'enum': - param_type = 'INT32' - for key in param['values']: - tags += '\n * @value {:} {:}'.format(key, param['values'][key]) - elif param['type'] == 'boolean': - param_type = 'INT32' - tags += '\n * @boolean' - elif param['type'] == 'int32': - param_type = 'INT32' - elif param['type'] == 'float': - param_type = 'FLOAT' - else: - raise Exception("unknown param type {:}".format(param['type'])) - - for tag in ['decimal', 'increment', 'category', 'volatile', 'bit', - 'min', 'max', 'unit', 'reboot_required']: - if tag in param: - tags += '\n * @{:} {:}'.format(tag, param[tag]) - - for i in range(num_instances): - # default value - default_value = 0 - if 'default' in param: - # default can be a list of num_instances or a single value - if type(param['default']) == list: - assert len(param['default']) == num_instances - default_value = param['default'][i] - else: - default_value = param['default'] - - if type(default_value) == bool: - default_value = int(default_value) - - # output the existing C-style format - ret += ''' -/** - * {short_descr} - * - * {long_descr} - * - * {tags} - */ -PARAM_DEFINE_{param_type}({name}, {default_value}); -'''.format(short_descr=param['description']['short'].replace("\n", "\n * "), - long_descr=param['description']['long'].replace("\n", "\n * "), - tags=tags, - param_type=param_type, - name=param_name, - default_value=default_value, - ).replace('${i}', str(i+instance_start)) - return ret - -def parse_yaml_ethernet_parameters_config(yaml_config): - """ parse the parameters section from the yaml config file """ - if 'parameters' not in yaml_config: - return '' - parameters_section_list = yaml_config['parameters'] - for parameters_section in parameters_section_list: - if 'definitions' not in parameters_section: - return '' - definitions = parameters_section['definitions'] - ret = '' - param_group = parameters_section.get('group', None) - for param_name in definitions: - param = definitions[param_name] - if 'ethernet' not in param: - continue - num_instances = param.get('num_instances', 1) - instance_start = param.get('instance_start', 0) # offset - - # get the type and extract all tags - tags = '@group {:}'.format(param_group) - if param['type'] == 'enum': - param_type = 'INT32' - for key in param['values']: - tags += '\n * @value {:} {:}'.format(key, param['values'][key]) - elif param['type'] == 'boolean': - param_type = 'INT32' - tags += '\n * @boolean' - elif param['type'] == 'int32': - param_type = 'INT32' - elif param['type'] == 'float': - param_type = 'FLOAT' - else: - raise Exception("unknown param type {:}".format(param['type'])) - - for tag in ['decimal', 'increment', 'category', 'volatile', 'bit', - 'min', 'max', 'unit', 'reboot_required']: - if tag in param: - tags += '\n * @{:} {:}'.format(tag, param[tag]) - - for i in range(num_instances): - # default value - default_value = 0 - if 'default' in param: - # default can be a list of num_instances or a single value - if type(param['default']) == list: - assert len(param['default']) == num_instances - default_value = param['default'][i] - else: - default_value = param['default'] - - if type(default_value) == bool: - default_value = int(default_value) - - # output the existing C-style format - ret += ''' -/** - * {short_descr} - * - * {long_descr} - * - * {tags} - */ -PARAM_DEFINE_{param_type}({name}, {default_value}); -'''.format(short_descr=param['description']['short'].replace("\n", "\n * "), - long_descr=param['description']['long'].replace("\n", "\n * "), - tags=tags, - param_type=param_type, - name=param_name, - default_value=default_value, - ).replace('${i}', str(i+instance_start)) - return ret - for yaml_file in args.config_files: with open(yaml_file, 'r') as stream: try: yaml_config = yaml.load(stream, Loader=yaml.Loader) serial_commands.extend(parse_yaml_serial_config(yaml_config)) - # TODO: additional params should be parsed in a separate script - additional_params += parse_yaml_parameters_config(yaml_config) - if ethernet_supported: - additional_ethernet_params += parse_yaml_ethernet_parameters_config(yaml_config) - except yaml.YAMLError as exc: print(exc) raise @@ -415,10 +268,14 @@ def parse_yaml_ethernet_parameters_config(yaml_config): default_port_str = port_config['default'][i] else: default_port_str = port_config['default'] + if default_port_str != "": if default_port_str not in serial_ports: raise Exception("Default Port {:} not found for {:}".format(default_port_str, serial_command['label'])) - default_port = serial_ports[default_port_str]['index'] + + if default_port_str in dict(board_ports).keys(): + default_port = serial_ports[default_port_str]['index'] + commands.append({ 'command': serial_command['command'], @@ -429,7 +286,7 @@ def parse_yaml_ethernet_parameters_config(yaml_config): 'default_port': default_port, 'param_group': port_config['group'], 'description_extended': port_config.get('description_extended', ''), - 'ethernet_config': serial_command.get('ethernet', 'none') + 'supports_networking': serial_command.get('supports_networking', False) }) if verbose: @@ -472,7 +329,5 @@ def parse_yaml_ethernet_parameters_config(yaml_config): with open(serial_params_output_file, 'w') as fid: fid.write(template.render(serial_devices=serial_devices, ethernet_configuration=ethernet_configuration, - commands=commands, serial_ports=serial_ports, - additional_definitions=additional_params, - additional_ethernet_definitions=additional_ethernet_params)) + commands=commands, serial_ports=serial_ports)) diff --git a/Tools/serial/rc.serial_port.jinja b/Tools/serial/rc.serial_port.jinja index c3f7c9651e18..087732176148 100644 --- a/Tools/serial/rc.serial_port.jinja +++ b/Tools/serial/rc.serial_port.jinja @@ -17,9 +17,9 @@ if param compare "$PRT" {{ serial_device.index }}; then fi {% endfor %} + {% for config in ethernet_configuration -%} if param compare "$PRT" {{ config.index }}; then set SERIAL_DEV ethernet fi - {% endfor %} diff --git a/Tools/serial/serial_params.c.jinja b/Tools/serial/serial_params.c.jinja index 4bed33a2c96d..13cee1e896ea 100644 --- a/Tools/serial/serial_params.c.jinja +++ b/Tools/serial/serial_params.c.jinja @@ -58,10 +58,11 @@ PARAM_DEFINE_INT32(SER_{{ serial_device.tag }}_BAUD, {{ serial_device.default_ba {%- for serial_device in serial_devices %} * @value {{ serial_device.index }} {{ serial_device.label }} {%- endfor %} -{%- if command.ethernet_config != "none" %} +{%- if command.supports_networking %} {%- for config in ethernet_configuration %} * @value {{ config.index }} {{ config.label }} -{%- endfor %}{% endif %} +{%- endfor %} +{%- endif %} * @group {{ command.param_group }} * @reboot_required true */ @@ -70,7 +71,3 @@ PARAM_DEFINE_INT32({{ command.port_param_name }}, {{ command.default_port }}); {% endfor -%} {% endif %} -{{ additional_definitions }} - -{{ additional_ethernet_definitions }} - diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh old mode 100644 new mode 100755 index 79d44496dce9..83bc225d495b --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -48,7 +48,6 @@ echo "Installing PX4 general dependencies" sudo pacman -Sy --noconfirm --needed \ astyle \ base-devel \ - ccache \ clang \ cmake \ cppcheck \ @@ -155,8 +154,7 @@ if [[ $INSTALL_SIM == "true" ]]; then # fix VMWare 3D graphics acceleration for gazebo exportline="export SVGA_VGPU10=0" - if grep -Fxq "$exportline" $HOME/.profile; then - else + if !grep -Fxq "$exportline" $HOME/.profile; then echo $exportline >> $HOME/.profile; fi fi diff --git a/Tools/setup/macos.sh b/Tools/setup/macos.sh index b0afb431f3c1..5133b66af033 100755 --- a/Tools/setup/macos.sh +++ b/Tools/setup/macos.sh @@ -33,6 +33,7 @@ if [[ $REINSTALL_FORMULAS == "--reinstall" ]]; then brew tap PX4/px4 brew reinstall px4-dev + brew install ncurses else if brew ls --versions px4-dev > /dev/null; then echo "px4-dev already installed" @@ -40,12 +41,13 @@ else echo "Installing PX4 general dependencies (homebrew px4-dev)" brew tap PX4/px4 brew install px4-dev + brew install ncurses fi fi # Python dependencies echo "Installing PX4 Python3 dependencies" -pip3 install --user -r ${DIR}/requirements.txt +python3 -m pip install --user -r ${DIR}/requirements.txt # Optional, but recommended additional simulation tools: if [[ $INSTALL_SIM == "--sim-tools" ]]; then diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index f7e322d14fde..e84c676d7e9b 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -3,10 +3,14 @@ argparse>=1.2 cerberus coverage empy>=3.3 +future jinja2>=2.8 +jsonschema +kconfiglib +lxml matplotlib>=3.0.* numpy>=1.13 -nunavut +nunavut>=1.1.0 packaging pandas>=0.21 pkgconfig @@ -15,7 +19,7 @@ pygments wheel>=0.31.1 pymavlink pyros-genmsg -pyserial>=3.0 +pyserial pyulog>=0.5.0 pyyaml requests diff --git a/Tools/setup/shell.nix b/Tools/setup/shell.nix index d3e77d3c47cc..0ade3f432af3 100644 --- a/Tools/setup/shell.nix +++ b/Tools/setup/shell.nix @@ -44,6 +44,7 @@ in pkgs.mkShell { coverage empy jinja2 + jsonschema matplotlib numpy packaging diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 933c77c46d0a..25c12cb170d8 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -1,5 +1,7 @@ #! /usr/bin/env bash +set -e + ## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04). ## Can also be used in docker. ## @@ -75,7 +77,6 @@ sudo apt-get update -y --quiet sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ astyle \ build-essential \ - ccache \ cmake \ cppcheck \ file \ @@ -84,6 +85,8 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i gdb \ git \ lcov \ + libxml2-dev \ + libxml2-utils \ make \ ninja-build \ python3 \ @@ -97,16 +100,16 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i zip \ ; -if [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then - echo "Installing Ubuntu 16.04 PX4-compatible ccache version" - wget -O /tmp/ccache_3.4.1-1_amd64.deb http://launchpadlibrarian.net/356662933/ccache_3.4.1-1_amd64.deb - sudo dpkg -i /tmp/ccache_3.4.1-1_amd64.deb -fi - # Python3 dependencies echo echo "Installing PX4 Python3 dependencies" -python3 -m pip install --user -r ${DIR}/requirements.txt +if [ -n "$VIRTUAL_ENV" ]; then + # virtual envrionments don't allow --user option + python -m pip install -r ${DIR}/requirements.txt +else + # older versions of Ubuntu require --user option + python3 -m pip install --user -r ${DIR}/requirements.txt +fi # NuttX toolchain (arm-none-eabi-gcc) if [[ $INSTALL_NUTTX == "true" ]]; then @@ -115,20 +118,40 @@ if [[ $INSTALL_NUTTX == "true" ]]; then echo "Installing NuttX dependencies" sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ - autoconf \ automake \ + binutils-dev \ bison \ - bzip2 \ - file \ + build-essential \ flex \ + g++-multilib \ + gcc-multilib \ gdb-multiarch \ + genromfs \ + gettext \ gperf \ - libncurses-dev \ + libelf-dev \ + libexpat-dev \ + libgmp-dev \ + libisl-dev \ + libmpc-dev \ + libmpfr-dev \ + libncurses5 \ + libncurses5-dev \ + libncursesw5-dev \ libtool \ pkg-config \ screen \ + texinfo \ + u-boot-tools \ + util-linux \ vim-common \ ; + if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; 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) @@ -177,13 +200,12 @@ if [[ $INSTALL_SIM == "true" ]]; then if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then java_version=11 - gazebo_version=9 elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then - java_version=14 - gazebo_version=11 + java_version=13 + elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then + java_version=11 else java_version=14 - gazebo_version=11 fi # Java (jmavsim or fastrtps) sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ @@ -197,20 +219,30 @@ if [[ $INSTALL_SIM == "true" ]]; then sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version") # Gazebo + if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then + gazebo_version=9 + gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev" + elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then + gazebo_packages="gazebo libgazebo-dev" + else + # default and Ubuntu 20.04 + gazebo_version=11 + gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev" + fi + 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 sudo apt-get update -y --quiet sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ dmidecode \ - gazebo$gazebo_version \ + $gazebo_packages \ gstreamer1.0-plugins-bad \ gstreamer1.0-plugins-base \ gstreamer1.0-plugins-good \ gstreamer1.0-plugins-ugly \ gstreamer1.0-libav \ libeigen3-dev \ - libgazebo$gazebo_version-dev \ libgstreamer-plugins-base1.0-dev \ libimage-exiftool-perl \ libopencv-dev \ diff --git a/Tools/setup_ignition.bash b/Tools/setup_ignition.bash new file mode 100644 index 000000000000..7def58d35848 --- /dev/null +++ b/Tools/setup_ignition.bash @@ -0,0 +1,23 @@ +#!/bin/bash +# +# Setup environment to make PX4 visible to Gazebo. +# +# Note, this is not necessary if using a ROS catkin workspace with the px4 +# package as the paths are exported. +# +# License: according to LICENSE.md in the root directory of the PX4 Firmware repository + +if [ "$#" != 2 ]; then + echo -e "usage: source setup_gazebo.bash src_dir build_dir\n" + return 1 +fi + +SRC_DIR=$1 +BUILD_DIR=$2 + +# setup Gazebo env and update package path +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$IGN_GAZEBO_SYSTEM_PLUGIN_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo +export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${SRC_DIR}/Tools/simulation-ignition/models + +echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH" diff --git a/Tools/simulation-ignition b/Tools/simulation-ignition new file mode 160000 index 000000000000..483193d9b8b8 --- /dev/null +++ b/Tools/simulation-ignition @@ -0,0 +1 @@ +Subproject commit 483193d9b8b89211c3b970c735b4fbb5f724b63a diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 3e5fed04d8e5..5610c3fb441a 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 3e5fed04d8e574b10e17e446d2938346bc6152ca +Subproject commit 5610c3fb441a2f3babc8ad7a63c8c4ce3e40abfa diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 9f36364ab22d..1ae1267b3df7 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -56,6 +56,13 @@ else follow_mode="" fi +# To use gazebo_ros ROS2 plugins +if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then + ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so" +else + ros_args="" +fi + if [ "$program" == "jmavsim" ]; then jmavsim_pid=`ps aux | grep java | grep "\-jar jmavsim_run.jar" | awk '{ print $2 }'` if [ -n "$jmavsim_pid" ]; then @@ -67,6 +74,9 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then if [ "$program" == "jsbsim" ]; then echo "empty model, setting rascal as default for jsbsim" model="rascal" + elif [ "$program" == "sihsim" ]; then + echo "empty model, setting quadx as default for sihsim" + model="quadx" else echo "empty model, setting iris as default" model="iris" @@ -133,7 +143,7 @@ elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then world_path="$PX4_SITL_WORLD" fi fi - gzserver $verbose $world_path & + gzserver $verbose $world_path $ros_args & SIM_PID=$! # Check all paths in ${GAZEBO_MODEL_PATH} for specified model @@ -177,6 +187,15 @@ elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then echo "You need to have gazebo simulator installed!" exit 1 fi +elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then + echo "Ignition Gazebo" + if [[ -n "$HEADLESS" ]]; then + ignition_headless="-s" + else + ignition_headless="" + fi + source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}" + ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"& elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then echo "FG setup" cd "${src_path}/Tools/flightgear_bridge/" @@ -198,6 +217,12 @@ elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then fi "${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" 2> /dev/null & JSBSIM_PID=$! +elif [ "$program" == "sihsim" ] && [ ! -n "$no_sim" ]; then + export SIM_MODE="sihsim" + if [ "$model" != "airplane" ] && [ "$model" != "quadx" ] && [ "$model" != "xvert" ]; then + echo "Model ${model} not compatible with with sih. sih supports [quadx,airplane,xvert]." + exit 1 + fi fi pushd "$rootfs" >/dev/null diff --git a/Tools/test_keys/key0.pub b/Tools/test_keys/key0.pub new file mode 100644 index 000000000000..86153ac96535 --- /dev/null +++ b/Tools/test_keys/key0.pub @@ -0,0 +1,5 @@ +//Public key to verify signed binaries +0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, +0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, +0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, +0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, diff --git a/Tools/test_keys/rsa2048.pem b/Tools/test_keys/rsa2048.pem new file mode 100644 index 000000000000..e2ee0e61bfde --- /dev/null +++ b/Tools/test_keys/rsa2048.pem @@ -0,0 +1,27 @@ +-----BEGIN RSA PRIVATE KEY----- +MIIEowIBAAKCAQEAz6O47E+sN+Yb+h6zkukagzYtJ6ZKq344b5N6H2CDg6z9LmG/ +GRAfOeNstsz1hXyyOCcUQ9f+vKHofTN0k+fWKy95h1mwY5Vc509gRsmpD96pNp4+ +NLp3GFDjawOTqsfyLQ7zKwU0YKT3qeI3o20n2KNxnGuyXt0SK3Ph2530wwzWBR2s +pNxpKvV8MCR7K4AbeuxodxKXKrLl0v9BHfUGpLZHpfoFfvChuK3eXK4si06tr6xt +RmLh6f6hxcn0SNKAK+WpPsH8mN4DqayGt7UZ5XVqY4M/J3pe+PFtstI2ocDSX5Oc +kOg+yonXytDH6I9Lt2CSXjZDcCM2+7WlBprAywIDAQABAoIBAGZE1nUV/NX/cXIt +IvdN9q//xBfOUOLMpVFXSwQfTkdRsdXhcPUQOsERYd9bbeZUd5cusE2GGkKgYFki +Od4LhzH4DRx8MWOrEnofX2UeODXHzoJHSI5B9Ry14n034shv+Lj2rxBWXOjo987l +y8+jmMecIP4REWal7igWyHyZ/Q7/5UCCCH5HaA4jgMhMdrD3zdxrlPhrW/egjfKF +Q+444qG5HzUqh7tOBxvf9Yu9FujHC9NxKDNXZZ3WBpwVQIVPCxexb4b8FBZ4syCH +iPbSgyw91CVfMvpWCYqDe7ubw4QDLHzDIiyKFtvHmIOgPo74y8fi1YYZ+3dy5mpa +UNdC11kCgYEA3whbNd71AVcSykRZ23AxDcjtUoiFXlpzSFes0QBGrmi2WTc04r8d +5Zr1E8oohc/zPJnUnCh93BAAu+HJsg4wKCqOWu5qR3NWcFp4/4voG9hsg1hgWD8Z +6fMMjoZRezSxadyeeaDStnfSkYyE2J8DPpkK5mjFG9IbrylGfApzDf0CgYEA7lTi +j2NQ+HNMlp/oOtsRZHkQtHYutv5SltG4E4DlwDHyA2ZLR2xB+D2ce5VdPQvfHu8F +ioMquDLeJbOPeikwko+RmpbmCsODjLueZHBmGwAfkuE0A5TZUwTi+W6iYbrXLK6E +TgNMaS4AGeBbioo6cdnMnYp9jdm63zvefet6oGcCgYAwr4BJmCvfaQR/BsCeuDTd +D3lOxOJoIFJ9/jWJQggr1kvH2dc/j/yUvGi3My/5VdWA6wuQMv6WZR/j43vF1HcK +rY95pgWpJzI9QGKdVgsK2QmG+mm9mbisaxPYoNV0kaIQu8oUPtkAX9OlVglByCRL +K9lHRqOQWSMV72qldRp8eQKBgHVCJkXN42SZtbDV8/ghGCmKtwFStCEsd43kmOBf +pqos6JlrltYJGVv9VCQplLoYQSqDBwLjDf2aaVm7QngkE9XH9SdN3tik4PA4zvEz +q8jVArPNQT4R2erSmKmIGTRkLMG7CzUmwk1taHdSvzcmUyL4uYc5QBSubxat6gWh ++a85AoGBAKgfnjjVoAWWvqEDLpfGPmE8lW+RaS7i5ff6QsSBx7uTEnHq6RNHuVnN +et4pR87yIENeG8jMBiDCj8AGDtUNt9Ps9vWCPrf9HSOYoBUk+gZagU/9N+RBpuCM +egoxtxIHM7HI+XIer+ZnZpVpgr+EoCaL7avx6k/susLQb7tqSBt1 +-----END RSA PRIVATE KEY----- \ No newline at end of file diff --git a/Tools/test_keys/rsa2048.pub b/Tools/test_keys/rsa2048.pub new file mode 100644 index 000000000000..57dfa81ce303 --- /dev/null +++ b/Tools/test_keys/rsa2048.pub @@ -0,0 +1,38 @@ + +0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9, +0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1, +0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0, +0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1, +0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37, +0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a, +0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e, +0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83, +0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f, +0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c, +0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc, +0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6, +0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95, +0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf, +0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77, +0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7, +0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60, +0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27, +0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd, +0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3, +0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69, +0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80, +0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a, +0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6, +0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0, +0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b, +0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1, +0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2, +0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98, +0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19, +0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a, +0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1, +0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e, +0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f, +0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70, +0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0, +0xcb, 0x2, 0x3, 0x1, 0x0, 0x1, diff --git a/Tools/test_keys.json b/Tools/test_keys/test_keys.json similarity index 100% rename from Tools/test_keys.json rename to Tools/test_keys/test_keys.json diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 844e4df3d7b8..34ef4a42046d 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -23,7 +23,7 @@ help='Excluded path(s), can be specified multiple times', default=[]) parser.add_argument('--merge-depends', action='store_true', - help='Merge library topics inte the modules that depend on them.') + help='Merge library topics in the modules that depend on them.') parser.add_argument('-v','--verbosity', action='count', help='increase output verbosity; primarily for debugging; repeat for more detail', default=0) @@ -56,6 +56,21 @@ def get_N_colors(N, s=0.8, v=0.9): hex_out.append("#"+"".join(map(lambda x: format(x, '02x'), rgb))) return hex_out +def topic_filename(topic): + MSG_PATH = 'msg/' + + file_list = os.listdir(MSG_PATH) + msg_files = [file.replace('.msg', '') for file in file_list if file.endswith(".msg")] + + if topic in msg_files: + return topic + else: + for msg_file in msg_files: + with open(f'{MSG_PATH}/{msg_file}.msg') as f: + ret = re.findall(f'^# TOPICS.*{topic}.*',f.read(),re.MULTILINE) + if len(ret) > 0: + return msg_file + return "no_file" class PubSub(object): """ Collects either publication or subscription information for nodes @@ -422,6 +437,7 @@ def _extract_build_information(self, file_name, **kwargs): found_module_def = False found_module_depends = False found_library_def = False + scope_added = False for line in datafile: if 'px4_add_module' in line: # must contain 'px4_add_module' found_module_def = True @@ -432,6 +448,7 @@ def _extract_build_information(self, file_name, **kwargs): library_name = tokens[1].split()[0].strip().rstrip(')') library_scope = LibraryScope(library_name) self._current_scope.append(library_scope) + scope_added = True self._found_libraries[library_name] = library_scope if self._in_scope(): log.debug(' >> found library: ' + library_name) @@ -443,16 +460,18 @@ def _extract_build_information(self, file_name, **kwargs): elif found_module_depends: # two tabs is a *sketchy* heuristic -- spacing isn't guaranteed by cmake; # ... but the hard-tabs *is* specified by PX4 coding standards, so it's likely to be consistent - if line.startswith('\t\t'): + if line.startswith('\t\t') and not line.strip().startswith('#'): depends = [dep.strip() for dep in line.split()] for name in depends: + log.debug(' >> {:}: found module dep: {:}' + .format(self._current_scope[-1].name, name)) self._current_scope[-1].add_dependency(name) if kwargs['merge_depends']: if (0 < len(self._scope_whitelist)) and self._current_scope[-1].name in self._scope_whitelist: # if we whitelist a module with dependencies, whitelist the dependencies, too self._scope_whitelist.add(name) - else: + elif line.strip() != "": found_module_depends = False ## done with the 'DEPENDS' section. words = line.split() @@ -461,17 +480,21 @@ def _extract_build_information(self, file_name, **kwargs): module_name = words[1] module_scope = ModuleScope(module_name) self._current_scope.append(module_scope) + scope_added = True self._found_modules[module_name] = module_scope if self._in_scope(): log.debug(' >> Found module name: ' + module_scope.name) - return (found_library_def or found_module_def) + return scope_added def _process_source_file(self, file_name): """ extract information from a single source file """ - log.debug( " >> extracting topics from file: " + file_name ) + current_scope = self._get_current_scope() + log.debug( " >> {:}extracting topics from file: {:}" + .format(current_scope.name+": " if current_scope is not None else "", + file_name)) with codecs.open(file_name, 'r', 'utf-8') as f: try: @@ -481,7 +504,6 @@ def _process_source_file(self, file_name): return - current_scope = self._get_current_scope() if current_scope: if current_scope.name in self._scope_blacklist: return @@ -672,8 +694,7 @@ def write(self, file_name): node['type'] = 'topic' node['color'] = topic_colors[topic] # url is opened when double-clicking on the node - # TODO: does not work for multi-topics - node['url'] = 'https://github.com/PX4/Firmware/blob/master/msg/'+topic+'.msg' + node['url'] = 'https://github.com/PX4/PX4-Autopilot/blob/master/msg/'+topic_filename(topic)+'.msg' nodes.append(node) data['nodes'] = nodes diff --git a/Tools/update_px4_ros2_bridge.sh b/Tools/update_px4_ros2_bridge.sh new file mode 100755 index 000000000000..58ebca5b7ce7 --- /dev/null +++ b/Tools/update_px4_ros2_bridge.sh @@ -0,0 +1,135 @@ +#!/usr/bin/env bash +set -e + +agent_template_files_updated=0 +code_generator_files_updated=0 + +# parse help argument +if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then + echo -e "Usage: update_px4_ros2_bridge.bash [options...] \t This script allows to update px4_ros_com and px4_msgs in a specified directory." >&2 + echo + echo -e "\t--ws_dir \t\t Location of the ament/colcon workspace. Default: $HOME/colcon_ws." + echo -e "\t--px4_ros_com \t\t Updates px4_ros_com microRTPS agent code generation and templates." + echo -e "\t--px4_msgs \t\t Updates px4_msgs messages definition files." + echo -e "\t--all \t\t Updates both px4_ros_com and px4_msgs." + echo + exit 0 +fi + +# parse the arguments +while [ $# -gt 0 ]; do + if [[ $1 == *"--"* ]]; then + v="${1/--/}" + if [ ! -z $2 ]; then + declare $v="$2" + else + declare $v=1 + fi + fi + shift +done + +# get script directory +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" + +# get PX4-Autopilot directory +PX4_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd) + +function compare_and_update () { + cmp -s "$1" "$2" + if [ $? -eq 1 ]; then + cp "$1" "$2" + return 0 + fi + + return 1; +} + +# update microRTPS agent code generators / helpers +function update_agent_code { + declare -a templates=( \ + "microRTPS_agent.cpp.em" \ + "microRTPS_timesync.cpp.em" \ + "microRTPS_timesync.h.em" \ + "microRTPS_transport.cpp" \ + "microRTPS_transport.h" \ + "Publisher.cpp.em" \ + "Publisher.h.em" \ + "Subscriber.cpp.em" \ + "Subscriber.h.em" \ + "RtpsTopics.cpp.em" \ + "RtpsTopics.h.em" \ + ) + + for file in ${templates[@]}; do + compare_and_update "$PX4_DIR/msg/templates/urtps/$file" "$ws_dir/src/px4_ros_com/templates/$file" \ + && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/templates/$file' updated" && ((agent_template_files_updated+=1)) + done + if [ $agent_template_files_updated -eq 0 ]; then + echo -e "--\t\t- No template files updated" + elif [ $agent_template_files_updated -eq 1 ]; then + echo -e "--\t\t - 1 template file updated" + else + echo -e "--\t\t - $agent_template_files_updated template files updated" + fi +} + +# update microRTPS agent code templates +function update_agent_templates { + declare -a code_generators=( \ + "uorb_rtps_classifier.py" \ + "generate_microRTPS_bridge.py" \ + "px_generate_uorb_topic_files.py" \ + ) + for file in ${code_generators[@]}; do + compare_and_update "$PX4_DIR/msg/tools/$file $ws_dir/src/px4_ros_com/scripts/$file" \ + && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/scripts/$file' updated" && ((code_generator_files_updated+=1)) + done + if [ $code_generator_files_updated -eq 0 ]; then + echo -e "--\t\t- No code generators / helpers files updated" + elif [ $code_generator_files_updated -eq 1 ]; then + echo -e "--\t\t - 1 code generator / helper file updated" + else + echo -e "--\t\t - $code_generator_files_updated code generator / helper files updated" + fi +} + +# update px4_ros_com files +function update_px4_ros_com { + python3 $PX4_DIR/msg/tools/uorb_to_ros_urtps_topics.py -i $PX4_DIR/msg/tools/urtps_bridge_topics.yaml -o $ws_dir/src/px4_ros_com/templates/urtps_bridge_topics.yaml + echo -e "--------------- \033[1mmicroRTPS agent code generation and templates update\033[0m ----------------" + echo "-------------------------------------------------------------------------------------------------------" + update_agent_code + update_agent_templates + return 0 +} + +# function to update px4_msgs +function update_px4_msgs { + find "$ws_dir/src/px4_msgs/msg/" -maxdepth 1 -type f -delete + python3 $PX4_DIR/msg/tools/uorb_to_ros_msgs.py $PX4_DIR/msg/ $ws_dir/src/px4_msgs/msg/ +} + +# decisor +echo "-------------------------------------------------------------------------------------------------------" +if [ -d "${ws_dir}" ]; then + ws_dir=$(cd "$ws_dir" && pwd) + if [ ! -z ${all} ]; then + update_px4_ros_com + echo "-------------------------------------------------------------------------------------------------------" + update_px4_msgs + elif [ ! -z ${px4_ros_com} ]; then + update_px4_ros_com + echo "-------------------------------------------------------------------------------------------------------" + elif [ ! -z ${px4_msgs} ]; then + update_px4_msgs + fi + echo -e "-------------------------------- \033[0;32mUpdate successful!\033[0m ---------------------------------" + echo "-------------------------------------------------------------------------------------------------------" + exit 0 +else + echo -e "-- \033[0;31mWorkspace directory doesn't exist...\033[0m" + echo -e "---------------------------------- \033[0;31mUpdate failed!\033[0m -----------------------------------" + echo "-------------------------------------------------------------------------------------------------------" + exit $ERRCODE +fi diff --git a/Tools/validate_json.py b/Tools/validate_json.py index 98affc3d37e5..b80dad3198be 100755 --- a/Tools/validate_json.py +++ b/Tools/validate_json.py @@ -5,6 +5,7 @@ import argparse import sys import json +import os try: from jsonschema import validate @@ -21,6 +22,8 @@ parser.add_argument('json_file', nargs='+', help='JSON config file(s)') parser.add_argument('--schema-file', type=str, action='store', help='JSON schema file', required=True) +parser.add_argument('--skip-if-no-schema', dest='skip_if_no_schema', action='store_true', + help='Skip test if schema file does not exist') parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help='Verbose Output') @@ -29,6 +32,9 @@ json_files = args.json_file verbose = args.verbose +if args.skip_if_no_schema and not os.path.isfile(schema_file): + sys.exit(0) + # load the schema with open(schema_file, 'r') as stream: schema = json.load(stream) diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake deleted file mode 100644 index 5ef2da1abb88..000000000000 --- a/boards/airmind/mindpx-v2/default.cmake +++ /dev/null @@ -1,133 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR airmind - MODEL mindpx-v2 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot - gps - #heater - #imu # all available imu drivers - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - imu/invensense/mpu6500 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board new file mode 100644 index 000000000000..7b52a99b646d --- /dev/null +++ b/boards/airmind/mindpx-v2/default.px4board @@ -0,0 +1,107 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y +CONFIG_DRIVERS_IMU_L3GD20=y +CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/airmind/mindpx-v2/bootloader/airmind_mindpx-v2_bootloader.bin b/boards/airmind/mindpx-v2/extras/airmind_mindpx-v2_bootloader.bin similarity index 100% rename from boards/airmind/mindpx-v2/bootloader/airmind_mindpx-v2_bootloader.bin rename to boards/airmind/mindpx-v2/extras/airmind_mindpx-v2_bootloader.bin diff --git a/boards/airmind/mindpx-v2/init/rc.board_defaults b/boards/airmind/mindpx-v2/init/rc.board_defaults new file mode 100644 index 000000000000..5d576dd74711 --- /dev/null +++ b/boards/airmind/mindpx-v2/init/rc.board_defaults @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 diff --git a/boards/airmind/mindpx-v2/init/rc.board_mavlink b/boards/airmind/mindpx-v2/init/rc.board_mavlink deleted file mode 100644 index 76e6351fb08c..000000000000 --- a/boards/airmind/mindpx-v2/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Airmind Mindpx-v2 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig index 19093de3c658..9f7d02d481d3 100644 --- a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig +++ b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/airmind/mindpx-v2/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -41,6 +42,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0030 CONFIG_CDCACM_PRODUCTSTR="MindPX FMU v2.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -90,18 +92,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,7 +114,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -134,8 +130,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -178,7 +173,7 @@ CONFIG_STM32_SPI1=y CONFIG_STM32_SPI2=y CONFIG_STM32_SPI4=y CONFIG_STM32_SPI4_DMA=y -CONFIG_STM32_SPI4_DMA_BUFFER=1024 +CONFIG_STM32_SPI4_DMA_BUFFER=512 CONFIG_STM32_SPI_DMA=y CONFIG_STM32_SPI_DMATHRESHOLD=8 CONFIG_STM32_TIM10=y diff --git a/boards/airmind/mindpx-v2/src/board_config.h b/boards/airmind/mindpx-v2/src/board_config.h index 0bb915f402eb..65d587f66ccc 100644 --- a/boards/airmind/mindpx-v2/src/board_config.h +++ b/boards/airmind/mindpx-v2/src/board_config.h @@ -72,12 +72,6 @@ #define ADC_RC_RSSI_CHANNEL 11 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - /* Power supply control and monitoring GPIOs */ // #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) // #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -95,7 +89,6 @@ /* AUX PWMs */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* USB OTG FS * @@ -153,7 +146,6 @@ #define BOARD_ADC_PERIPH_5V_OC (0) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/airmind/mindpx-v2/src/init.c b/boards/airmind/mindpx-v2/src/init.c index 90415bceb50c..30a3e6a67900 100644 --- a/boards/airmind/mindpx-v2/src/init.c +++ b/boards/airmind/mindpx-v2/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -217,22 +218,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -249,7 +239,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi4) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 4\n"); board_autoled_on(LED_AMBER); - return -ENODEV; } /* Default SPI4 to 10MHz and de-assert the known chip selects. */ @@ -265,7 +254,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); board_autoled_on(LED_AMBER); - return -ENODEV; } /* Default SPI1 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) @@ -289,9 +277,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); } /* Now bind the SDIO interface to the MMC/SD driver */ @@ -299,7 +285,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ diff --git a/boards/ark/can-flow/canbootloader.cmake b/boards/ark/can-flow/canbootloader.cmake deleted file mode 100644 index 1ca01c65a606..000000000000 --- a/boards/ark/can-flow/canbootloader.cmake +++ /dev/null @@ -1,13 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR ark - MODEL can-flow - LABEL canbootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - DRIVERS - bootloaders -) diff --git a/boards/ark/can-flow/canbootloader.px4board b/boards/ark/can-flow/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/ark/can-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/ark/can-flow/debug.cmake b/boards/ark/can-flow/debug.cmake deleted file mode 100644 index ddcb65e86744..000000000000 --- a/boards/ark/can-flow/debug.cmake +++ /dev/null @@ -1,33 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR ark - MODEL can-flow - LABEL debug - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 1 - DRIVERS - bootloaders - imu/bosch/bmi088 - optical_flow/paw3902 - uavcannode - MODULES - #ekf2 - load_mon - #sensors - SYSTEMCMDS - mft - mtd - param - perf - reboot - system_time - top - topic_listener - ver - work_queue -) diff --git a/boards/ark/can-flow/default.cmake b/boards/ark/can-flow/default.cmake deleted file mode 100644 index 7ac68b3f10ce..000000000000 --- a/boards/ark/can-flow/default.cmake +++ /dev/null @@ -1,34 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR ark - MODEL can-flow - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_FLASH - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 1 - DRIVERS - bootloaders - imu/bosch/bmi088 - optical_flow/paw3902 - uavcannode - MODULES - #ekf2 - #load_mon - #sensors - SYSTEMCMDS - mft - mtd - param - #perf - #reboot - #system_time - #top - #topic_listener - #ver - #work_queue -) diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board new file mode 100644 index 000000000000..48b3132cc3d9 --- /dev/null +++ b/boards/ark/can-flow/default.px4board @@ -0,0 +1,26 @@ +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_OPTICAL_FLOW_PAW3902=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=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_GPS_POSITION is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=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/ark/can-flow/init/rc.board_sensors b/boards/ark/can-flow/init/rc.board_sensors index 7750a31898b9..b7502f98fde5 100644 --- a/boards/ark/can-flow/init/rc.board_sensors +++ b/boards/ark/can-flow/init/rc.board_sensors @@ -3,7 +3,15 @@ # board sensors init #------------------------------------------------------------------------------ +param set-default IMU_GYRO_RATEMAX 1000 + # Internal SPI -paw3902 -s start -Y 90 -bmi088 -A -s -R 4 start -bmi088 -G -s -R 4 start +paw3902 -s start -Y 180 + +if ! icm42688p -R 0 -s start +then + bmi088 -A -s -R 4 start + bmi088 -G -s -R 4 start +fi + +afbrs50 start diff --git a/boards/ark/can-flow/nuttx-config/canbootloader/defconfig b/boards/ark/can-flow/nuttx-config/canbootloader/defconfig index 40dffb67e84d..75230422cf17 100644 --- a/boards/ark/can-flow/nuttx-config/canbootloader/defconfig +++ b/boards/ark/can-flow/nuttx-config/canbootloader/defconfig @@ -7,7 +7,7 @@ # CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -36,17 +36,10 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=0 +CONFIG_FS_PROCFS_MAX_TASKS=0 CONFIG_MM_REGIONS=2 CONFIG_NAME_MAX=0 CONFIG_NUNGET_CHARS=0 -CONFIG_NXFONTS_DISABLE_16BPP=y -CONFIG_NXFONTS_DISABLE_1BPP=y -CONFIG_NXFONTS_DISABLE_24BPP=y -CONFIG_NXFONTS_DISABLE_2BPP=y -CONFIG_NXFONTS_DISABLE_32BPP=y -CONFIG_NXFONTS_DISABLE_4BPP=y -CONFIG_NXFONTS_DISABLE_8BPP=y CONFIG_PREALLOC_TIMERS=0 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=262144 diff --git a/boards/ark/can-flow/nuttx-config/include/board.h b/boards/ark/can-flow/nuttx-config/include/board.h index cd6ea967dbb2..09db41dead05 100644 --- a/boards/ark/can-flow/nuttx-config/include/board.h +++ b/boards/ark/can-flow/nuttx-config/include/board.h @@ -124,17 +124,13 @@ #define GPIO_CAN1_RX GPIO_CAN1_RX_2 #define GPIO_CAN1_TX GPIO_CAN1_TX_2 -/* I2C */ -#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 -#define GPIO_I2C1_SDA GPIO_I2C1_SDA_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 -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_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/nuttx-config/include/board_dma_map.h b/boards/ark/can-flow/nuttx-config/include/board_dma_map.h index 2d52cfbfec2a..0eb81fc589d6 100644 --- a/boards/ark/can-flow/nuttx-config/include/board_dma_map.h +++ b/boards/ark/can-flow/nuttx-config/include/board_dma_map.h @@ -35,11 +35,10 @@ // 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_SPI2_RX DMAMAP_SPI2_RX // DMA2, Stream 3, Channel 0 -#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA2, Stream 4, Channel 0 -#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 5, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 // DMA2, Stream 5, Channel 3 diff --git a/boards/ark/can-flow/nuttx-config/nsh/defconfig b/boards/ark/can-flow/nuttx-config/nsh/defconfig index de632fc595ff..7c8691089f10 100644 --- a/boards/ark/can-flow/nuttx-config/nsh/defconfig +++ b/boards/ark/can-flow/nuttx-config/nsh/defconfig @@ -15,12 +15,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_DMACAPABLE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -56,8 +57,6 @@ 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_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y @@ -70,18 +69,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -97,23 +91,21 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y @@ -130,7 +122,6 @@ CONFIG_STM32_DMA1=y CONFIG_STM32_DMA2=y CONFIG_STM32_FLASH_PREFETCH=y CONFIG_STM32_FLOWCONTROL_BROKEN=y -CONFIG_STM32_I2C1=y CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_PWR=y CONFIG_STM32_RTC=y @@ -142,10 +133,10 @@ 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_SPI1_DMA_BUFFER=2048 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI2_DMA=y -CONFIG_STM32_SPI2_DMA_BUFFER=1024 +CONFIG_STM32_SPI2_DMA_BUFFER=2048 CONFIG_STM32_SPI_DMA=y CONFIG_STM32_TIM8=y CONFIG_STM32_USART2=y diff --git a/boards/ark/can-flow/src/CMakeLists.txt b/boards/ark/can-flow/src/CMakeLists.txt index 0a0d16082306..4fae41fc0ecb 100644 --- a/boards/ark/can-flow/src/CMakeLists.txt +++ b/boards/ark/can-flow/src/CMakeLists.txt @@ -49,7 +49,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") else() add_library(drivers_board can.c - i2c.cpp init.c led.c spi.cpp @@ -62,6 +61,5 @@ else() nuttx_arch nuttx_drivers px4_layer - arch_io_pins ) endif() diff --git a/boards/ark/can-flow/src/board_config.h b/boards/ark/can-flow/src/board_config.h index 44424aeae78f..40f302e19b79 100644 --- a/boards/ark/can-flow/src/board_config.h +++ b/boards/ark/can-flow/src/board_config.h @@ -48,6 +48,7 @@ /* 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) @@ -56,6 +57,13 @@ #define GPIO_nLED_RED /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) #define GPIO_nLED_BLUE /* PA8 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#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 BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED #define BOARD_ARMED_STATE_LED LED_BLUE diff --git a/boards/ark/can-flow/src/init.c b/boards/ark/can-flow/src/init.c index 501c13d29d73..8b1662d2d28c 100644 --- a/boards/ark/can-flow/src/init.c +++ b/boards/ark/can-flow/src/init.c @@ -49,6 +49,7 @@ #include #include #include +#include #include @@ -60,6 +61,7 @@ #include #include +#include #include @@ -87,6 +89,8 @@ __END_DECLS __EXPORT void stm32_boardinitialize(void) { + watchdog_init(); + // Configure CAN interface stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); @@ -142,7 +146,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); - return -ENODEV; } #endif // FLASH_BASED_PARAMS diff --git a/boards/ark/can-flow/src/spi.cpp b/boards/ark/can-flow/src/spi.cpp index d517d36c28ac..92d24684a885 100644 --- a/boards/ark/can-flow/src/spi.cpp +++ b/boards/ark/can-flow/src/spi.cpp @@ -39,6 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), initSPIDevice(DRV_FLOW_DEVTYPE_PAW3902, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), }), initSPIBus(SPI::Bus::SPI2, { diff --git a/boards/ark/can-gps/canbootloader.px4board b/boards/ark/can-gps/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/ark/can-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/can-gps/default.px4board b/boards/ark/can-gps/default.px4board new file mode 100644 index 000000000000..8f69c01983e1 --- /dev/null +++ b/boards/ark/can-gps/default.px4board @@ -0,0 +1,30 @@ +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_BAROMETER_BMP388=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=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/ark/can-gps/firmware.prototype b/boards/ark/can-gps/firmware.prototype new file mode 100644 index 000000000000..d7b32b3bd539 --- /dev/null +++ b/boards/ark/can-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 81, + "magic": "PX4FWv1", + "description": "Firmware for the ARK gps board", + "image": "", + "build_time": 0, + "summary": "ARKGPS", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults new file mode 100644 index 000000000000..480d2c92c58d --- /dev/null +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -0,0 +1,10 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default CBRK_IO_SAFETY 0 +param set-default MBE_ENABLE 1 + +safety_button start +tone_alarm start diff --git a/boards/ark/can-gps/init/rc.board_sensors b/boards/ark/can-gps/init/rc.board_sensors new file mode 100644 index 000000000000..d74d5f74b24d --- /dev/null +++ b/boards/ark/can-gps/init/rc.board_sensors @@ -0,0 +1,11 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ +gps start -d /dev/ttyS0 -p ubx + +icm42688p -R 0 -s start + +bmp388 -I -b 2 start + +bmm150 -I -b 1 start diff --git a/boards/ark/can-gps/nuttx-config/canbootloader/defconfig b/boards/ark/can-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..8d495caa8dae --- /dev/null +++ b/boards/ark/can-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,61 @@ +# +# 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-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_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=0 +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_SDCLONE_DISABLE=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 +CONFIG_USERMAIN_STACKSIZE=4096 diff --git a/boards/ark/can-gps/nuttx-config/include/board.h b/boards/ark/can-gps/nuttx-config/include/board.h new file mode 100644 index 000000000000..526392b92bbd --- /dev/null +++ b/boards/ark/can-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/boards/ark/can-gps/nuttx-config/include/board_dma_map.h b/boards/ark/can-gps/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..f6e577b3d5fe --- /dev/null +++ b/boards/ark/can-gps/nuttx-config/include/board_dma_map.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 +//--------------------------------------------//---------------------------//---------------- + + +// 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 5, Channel 3 diff --git a/boards/ark/can-gps/nuttx-config/nsh/defconfig b/boards/ark/can-gps/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..c305c085f48b --- /dev/null +++ b/boards/ark/can-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,158 @@ +# +# 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_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-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=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=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=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_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +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_SPI_DMA=y +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=600 +CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/ark/can-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/can-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 000000000000..cbf9fddc32dc --- /dev/null +++ b/boards/ark/can-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 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. + */ + +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-gps/nuttx-config/scripts/script.ld b/boards/ark/can-gps/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c63c74994199 --- /dev/null +++ b/boards/ark/can-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 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. + */ + +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/ark/can-gps/src/CMakeLists.txt b/boards/ark/can-gps/src/CMakeLists.txt new file mode 100644 index 000000000000..06bd98156f22 --- /dev/null +++ b/boards/ark/can-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/can-gps/src/board_config.h b/boards/ark/can-gps/src/board_config.h new file mode 100644 index 000000000000..723310472f4a --- /dev/null +++ b/boards/ark/can-gps/src/board_config.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 + +/* BUTTON */ +#define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) + +/* Safety LED */ +#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) + +/* 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_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_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ + GPIO_I2C1_SCL_RESET, \ + GPIO_I2C1_SDA_RESET, \ + GPIO_I2C2_SCL_RESET, \ + GPIO_I2C2_SDA_RESET, \ + 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/can-gps/src/boot.c b/boards/ark/can-gps/src/boot.c new file mode 100644 index 000000000000..a26034e254f6 --- /dev/null +++ b/boards/ark/can-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/can-gps/src/boot_config.h b/boards/ark/can-gps/src/boot_config.h new file mode 100644 index 000000000000..eab3c76c6c13 --- /dev/null +++ b/boards/ark/can-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 5000 +#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 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_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/can-gps/src/can.c b/boards/ark/can-gps/src/can.c new file mode 100644 index 000000000000..acc20252fab0 --- /dev/null +++ b/boards/ark/can-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_arch.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-gps/src/i2c.cpp b/boards/ark/can-gps/src/i2c.cpp new file mode 100644 index 000000000000..76486af73317 --- /dev/null +++ b/boards/ark/can-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), + initI2CBusInternal(2), +}; diff --git a/boards/ark/can-gps/src/init.c b/boards/ark/can-gps/src/init.c new file mode 100644 index 000000000000..f6b32dd8d107 --- /dev/null +++ b/boards/ark/can-gps/src/init.c @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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 button is held. If so go into gps passthrough mode + if (stm32_gpioread(GPIO_BTN_SAFETY)) { + 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)); + } + } +} + +/**************************************************************************** + * 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-gps/src/led.c b/boards/ark/can-gps/src/led.c new file mode 100644 index 000000000000..9a80cae08923 --- /dev/null +++ b/boards/ark/can-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/can-gps/src/led.h b/boards/ark/can-gps/src/led.h new file mode 100644 index 000000000000..b68e4aa70d0c --- /dev/null +++ b/boards/ark/can-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/can-gps/src/spi.cpp b/boards/ark/can-gps/src/spi.cpp new file mode 100644 index 000000000000..baafb0354c6c --- /dev/null +++ b/boards/ark/can-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/can-gps/uavcan_board_identity b/boards/ark/can-gps/uavcan_board_identity new file mode 100644 index 000000000000..ca6d098e5942 --- /dev/null +++ b/boards/ark/can-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +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 81) +set(uavcanblid_name "\"org.ark.can-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/ark/can-rtk-gps/canbootloader.px4board b/boards/ark/can-rtk-gps/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/debug.px4board b/boards/ark/can-rtk-gps/debug.px4board new file mode 100644 index 000000000000..dee7679749cc --- /dev/null +++ b/boards/ark/can-rtk-gps/debug.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_CONSTRAINED_FLASH=n +CONFIG_BOARD_NO_HELP=n +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board new file mode 100644 index 000000000000..8f69c01983e1 --- /dev/null +++ b/boards/ark/can-rtk-gps/default.px4board @@ -0,0 +1,30 @@ +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_BAROMETER_BMP388=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=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/ark/can-rtk-gps/firmware.prototype b/boards/ark/can-rtk-gps/firmware.prototype new file mode 100644 index 000000000000..f8a96f3e7bf2 --- /dev/null +++ b/boards/ark/can-rtk-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 82, + "magic": "PX4FWv1", + "description": "Firmware for the ARK RTK GPS board", + "image": "", + "build_time": 0, + "summary": "ARKRTKGPS", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults new file mode 100644 index 000000000000..25bc347662c6 --- /dev/null +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -0,0 +1,11 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default CBRK_IO_SAFETY 0 +param set-default CANNODE_GPS_RTCM 1 +param set-default MBE_ENABLE 1 + +safety_button start +tone_alarm start diff --git a/boards/ark/can-rtk-gps/init/rc.board_sensors b/boards/ark/can-rtk-gps/init/rc.board_sensors new file mode 100644 index 000000000000..d74d5f74b24d --- /dev/null +++ b/boards/ark/can-rtk-gps/init/rc.board_sensors @@ -0,0 +1,11 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ +gps start -d /dev/ttyS0 -p ubx + +icm42688p -R 0 -s start + +bmp388 -I -b 2 start + +bmm150 -I -b 1 start diff --git a/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig b/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..1ce0048f92ff --- /dev/null +++ b/boards/ark/can-rtk-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,61 @@ +# +# 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-rtk-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_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=0 +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_SDCLONE_DISABLE=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 +CONFIG_USERMAIN_STACKSIZE=4096 diff --git a/boards/ark/can-rtk-gps/nuttx-config/include/board.h b/boards/ark/can-rtk-gps/nuttx-config/include/board.h new file mode 100644 index 000000000000..526392b92bbd --- /dev/null +++ b/boards/ark/can-rtk-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/boards/ark/can-rtk-gps/nuttx-config/include/board_dma_map.h b/boards/ark/can-rtk-gps/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..efa3d824b2f6 --- /dev/null +++ b/boards/ark/can-rtk-gps/nuttx-config/include/board_dma_map.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 +//--------------------------------------------//---------------------------//---------------- + + +// 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/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..b669e9646327 --- /dev/null +++ b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,159 @@ +# +# 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_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-rtk-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=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=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=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_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +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_SPI_DMA=y +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 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/ark/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 000000000000..cbf9fddc32dc --- /dev/null +++ b/boards/ark/can-rtk-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 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. + */ + +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-rtk-gps/nuttx-config/scripts/script.ld b/boards/ark/can-rtk-gps/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c63c74994199 --- /dev/null +++ b/boards/ark/can-rtk-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 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. + */ + +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/ark/can-rtk-gps/src/CMakeLists.txt b/boards/ark/can-rtk-gps/src/CMakeLists.txt new file mode 100644 index 000000000000..06bd98156f22 --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/src/board_config.h b/boards/ark/can-rtk-gps/src/board_config.h new file mode 100644 index 000000000000..2263956bae0f --- /dev/null +++ b/boards/ark/can-rtk-gps/src/board_config.h @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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 + +/* BUTTON */ +#define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) + +/* Safety LED */ +#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) + +/* 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_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_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_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ + GPIO_I2C1_SCL_RESET, \ + GPIO_I2C1_SDA_RESET, \ + GPIO_I2C2_SCL_RESET, \ + GPIO_I2C2_SDA_RESET, \ + 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/can-rtk-gps/src/boot.c b/boards/ark/can-rtk-gps/src/boot.c new file mode 100644 index 000000000000..a26034e254f6 --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/src/boot_config.h b/boards/ark/can-rtk-gps/src/boot_config.h new file mode 100644 index 000000000000..eab3c76c6c13 --- /dev/null +++ b/boards/ark/can-rtk-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 5000 +#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 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_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/can-rtk-gps/src/can.c b/boards/ark/can-rtk-gps/src/can.c new file mode 100644 index 000000000000..acc20252fab0 --- /dev/null +++ b/boards/ark/can-rtk-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_arch.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-rtk-gps/src/i2c.cpp b/boards/ark/can-rtk-gps/src/i2c.cpp new file mode 100644 index 000000000000..76486af73317 --- /dev/null +++ b/boards/ark/can-rtk-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), + initI2CBusInternal(2), +}; diff --git a/boards/ark/can-rtk-gps/src/init.c b/boards/ark/can-rtk-gps/src/init.c new file mode 100644 index 000000000000..7405575781e5 --- /dev/null +++ b/boards/ark/can-rtk-gps/src/init.c @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * 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 button is held. If so go into gps passthrough mode + if (stm32_gpioread(GPIO_BTN_SAFETY)) { + 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)); + } + } +} + +/**************************************************************************** + * 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/can-rtk-gps/src/led.c b/boards/ark/can-rtk-gps/src/led.c new file mode 100644 index 000000000000..9a80cae08923 --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/src/led.h b/boards/ark/can-rtk-gps/src/led.h new file mode 100644 index 000000000000..b68e4aa70d0c --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/src/spi.cpp b/boards/ark/can-rtk-gps/src/spi.cpp new file mode 100644 index 000000000000..baafb0354c6c --- /dev/null +++ b/boards/ark/can-rtk-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/can-rtk-gps/uavcan_board_identity b/boards/ark/can-rtk-gps/uavcan_board_identity new file mode 100644 index 000000000000..5db21a4ffb03 --- /dev/null +++ b/boards/ark/can-rtk-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +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 82) +set(uavcanblid_name "\"org.ark.can-rtk-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/ark/cannode/canbootloader.px4board b/boards/ark/cannode/canbootloader.px4board new file mode 100644 index 000000000000..51093ca3386c --- /dev/null +++ b/boards/ark/cannode/canbootloader.px4board @@ -0,0 +1,6 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_NSH_BUILTIN_APPS=y diff --git a/boards/ark/cannode/default.px4board b/boards/ark/cannode/default.px4board new file mode 100644 index 000000000000..14493945baf2 --- /dev/null +++ b/boards/ark/cannode/default.px4board @@ -0,0 +1,37 @@ +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_BOARD_EXTERNAL_METADATA=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_COMMON_HYGROMETERS=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +#CONFIG_MODULES_SENSORS=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/cannode/firmware.prototype b/boards/ark/cannode/firmware.prototype new file mode 100644 index 000000000000..8b149d8c5f8b --- /dev/null +++ b/boards/ark/cannode/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 83, + "magic": "PX4FWv1", + "description": "Firmware for the ARK CANnode board", + "image": "", + "build_time": 0, + "summary": "ARKCANNODE", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults new file mode 100644 index 000000000000..ba6faf3d0a49 --- /dev/null +++ b/boards/ark/cannode/init/rc.board_defaults @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +pwm_out start + +control_allocator start diff --git a/boards/ark/cannode/init/rc.board_sensors b/boards/ark/cannode/init/rc.board_sensors new file mode 100644 index 000000000000..bbddf5f5d49a --- /dev/null +++ b/boards/ark/cannode/init/rc.board_sensors @@ -0,0 +1,118 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ +icm42688p -R 0 -s start + +if param compare -s SENS_EN_BATT 1 +then + batt_smbus start -X +fi + +# Lidar-Lite on I2C +if param compare -s SENS_EN_LL40LS 2 +then + ll40ls start -X +fi + +# mappydot lidar sensor +if param compare -s SENS_EN_MPDT 1 +then + mappydot start -X +fi + +# mb12xx sonar sensor +if param greater -s SENS_EN_MB12XX 0 +then + mb12xx start -X +fi + +# Lightware i2c lidar sensor +if param greater -s SENS_EN_SF1XX 0 +then + lightware_laser_i2c start -X +fi + +# vl53l1x i2c distance sensor +if param compare -s SENS_EN_VL53L1X 1 +then + vl53l1x start -X +fi + +# ADIS16448 spi external IMU +if param compare -s SENS_EN_ADIS164X 1 +then + if param compare -s SENS_OR_ADIS164X 0 + then + adis16448 -S start + fi + if param compare -s SENS_OR_ADIS164X 4 + then + adis16448 -S start -R 4 + fi +fi + +# Eagle Tree airspeed sensor external I2C +if param compare -s SENS_EN_ETSASPD 1 +then + ets_airspeed start -X +fi + +# Sensirion SDP3X differential pressure sensor external I2C +if param compare -s SENS_EN_SDP3X 1 +then + if ! sdp3x start -X + then + # try another common address + sdp3x start -X -a 0x22 + fi +fi + +# SHT3x temperature and hygrometer sensor, external I2C +if param compare -s SENS_EN_SHT3X 1 +then + sht3x start -X + sht3x start -X -a 0x45 +fi + +# TE MS4525DO differential pressure sensor external I2C +if param compare -s SENS_EN_MS4525DO 1 +then + ms4525do start -X +fi + +# TE MS5525DSO differential pressure sensor external I2C +if param compare -s SENS_EN_MS5525DS 1 +then + ms5525dso start -X +fi + +# IR-LOCK sensor external I2C +if param compare -s SENS_EN_IRLOCK 1 +then + irlock start -X +fi + +# SPL06 sensor external I2C +if param compare -s SENS_EN_SPL06 1 +then + spl06 -X start + spl06 -X -a 0x77 start +fi + +gps start -d /dev/ttyS0 -p ubx + +# probe for optional external I2C devices +icm20948_i2c_passthrough -X -q start + +# compasses +hmc5883 -T -X -q start +ist8308 -X -q start +ist8310 -X -q start +lis2mdl -X -q start +lis3mdl -X -q start +qmc5883l -X -q start +rm3100 -X -q start + +# start last (wait for possible icm20948 passthrough mode) +ak09916 -X -q start diff --git a/boards/ark/cannode/nuttx-config/canbootloader/defconfig b/boards/ark/cannode/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..2e99504e20a9 --- /dev/null +++ b/boards/ark/cannode/nuttx-config/canbootloader/defconfig @@ -0,0 +1,61 @@ +# +# 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/cannode/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_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=0 +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_SDCLONE_DISABLE=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_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=4096 diff --git a/boards/ark/cannode/nuttx-config/include/board.h b/boards/ark/cannode/nuttx-config/include/board.h new file mode 100644 index 000000000000..aa067118540a --- /dev/null +++ b/boards/ark/cannode/nuttx-config/include/board.h @@ -0,0 +1,149 @@ +/************************************************************************************ + * 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_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_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 + +/* 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_2 /* PB13 */ + +/* 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_2 + +#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) + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/ark/cannode/nuttx-config/include/board_dma_map.h b/boards/ark/cannode/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..0eb81fc589d6 --- /dev/null +++ b/boards/ark/cannode/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/cannode/nuttx-config/nsh/defconfig b/boards/ark/cannode/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..5b6364324ee7 --- /dev/null +++ b/boards/ark/cannode/nuttx-config/nsh/defconfig @@ -0,0 +1,160 @@ +# +# 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_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/cannode/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=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=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=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_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +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_SPI_DMA=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=600 +CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/ark/cannode/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/cannode/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 000000000000..cbf9fddc32dc --- /dev/null +++ b/boards/ark/cannode/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. + */ + +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/cannode/nuttx-config/scripts/script.ld b/boards/ark/cannode/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..daf33d4b97b0 --- /dev/null +++ b/boards/ark/cannode/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 STM32F412CG has 1Mb 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/cannode/src/CMakeLists.txt b/boards/ark/cannode/src/CMakeLists.txt new file mode 100644 index 000000000000..2f1b3015920e --- /dev/null +++ b/boards/ark/cannode/src/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# 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 + timer_config.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/cannode/src/board_config.h b/boards/ark/cannode/src/board_config.h new file mode 100644 index 000000000000..bb820a7b66f3 --- /dev/null +++ b/boards/ark/cannode/src/board_config.h @@ -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 board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PC14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PC15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PH1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTH|GPIO_PIN1|GPIO_EXTI) + +/* ICM42688p FSYNC */ +#define GPIO_42688P_FSYNC /* PB8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* 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) + +/* PWM Outputs */ +#define DIRECT_PWM_OUTPUT_CHANNELS 6 // Actually 8 + +#define GPIO_TIM2_CH1_RESET /* PA0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TIM2_CH2_RESET /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_TIM2_CH3_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_TIM3_CH1_RESET /* PB4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_TIM3_CH2_RESET /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_TIM3_CH3_RESET /* PB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_TIM3_CH4_RESET /* PB1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +#define GPIO_TIM4_CH4_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +#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 /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_USART1_RX_GPIO /* PB3 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3) +#define GPIO_USART1_TX_GPIO /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_USART2_RX_GPIO /* PA3 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3) +#define GPIO_USART2_TX_GPIO /* PA2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2) + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer 8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + GPIO_42688P_FSYNC, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_I2C1_SCL_RESET, \ + GPIO_I2C1_SDA_RESET, \ + } + +__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/cannode/src/boot.c b/boards/ark/cannode/src/boot.c new file mode 100644 index 000000000000..a26034e254f6 --- /dev/null +++ b/boards/ark/cannode/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/cannode/src/boot_config.h b/boards/ark/cannode/src/boot_config.h new file mode 100644 index 000000000000..eab3c76c6c13 --- /dev/null +++ b/boards/ark/cannode/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 5000 +#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 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_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/cannode/src/can.c b/boards/ark/cannode/src/can.c new file mode 100644 index 000000000000..acc20252fab0 --- /dev/null +++ b/boards/ark/cannode/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_arch.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/src/i2c.cpp b/boards/ark/cannode/src/i2c.cpp similarity index 100% rename from boards/ark/can-flow/src/i2c.cpp rename to boards/ark/cannode/src/i2c.cpp diff --git a/boards/ark/cannode/src/init.c b/boards/ark/cannode/src/init.c new file mode 100644 index 000000000000..491bc348b402 --- /dev/null +++ b/boards/ark/cannode/src/init.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * 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 + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * 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) +{ + // Configure the GPIO pins to outputs and keep them low. + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + 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. + */ + + if (status >= 0) { + up_mdelay(400); + } +} + +/************************************************************************************ + * 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 all PWM to Low outputs. + board_on_reset(-1); + + 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(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/cannode/src/led.c b/boards/ark/cannode/src/led.c new file mode 100644 index 000000000000..9a80cae08923 --- /dev/null +++ b/boards/ark/cannode/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/cannode/src/led.h b/boards/ark/cannode/src/led.h new file mode 100644 index 000000000000..b68e4aa70d0c --- /dev/null +++ b/boards/ark/cannode/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/cannode/src/spi.cpp b/boards/ark/cannode/src/spi.cpp new file mode 100644 index 000000000000..ab7d82a3d831 --- /dev/null +++ b/boards/ark/cannode/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_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBusExternal(SPI::Bus::SPI2, { + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin12}), + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin13}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/cannode/src/timer_config.cpp b/boards/ark/cannode/src/timer_config.cpp new file mode 100644 index 000000000000..bc69d4b7ea1b --- /dev/null +++ b/boards/ark/cannode/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer2), + initIOTimer(Timer::Timer3), + //initIOTimer(Timer::Timer4), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortB, GPIO::Pin4}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortB, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + //initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + //initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortB, GPIO::Pin7}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/ark/cannode/uavcan_board_identity b/boards/ark/cannode/uavcan_board_identity new file mode 100644 index 000000000000..44ea18cc7d39 --- /dev/null +++ b/boards/ark/cannode/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +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 83) +set(uavcanblid_name "\"org.ark.cannode\"") + +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/atl/mantis-edu/cmake/upload.cmake b/boards/atl/mantis-edu/cmake/upload.cmake new file mode 100644 index 000000000000..b85bacd27957 --- /dev/null +++ b/boards/atl/mantis-edu/cmake/upload.cmake @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4) + +# The file needs to have that name in order to be updated automatically. +set(UPLOAD_NAME autopilot.px4) + +add_custom_target(upload_wifi + COMMAND ${CMAKE_COMMAND} -E copy ${PX4_FW_NAME} ${UPLOAD_NAME} + COMMAND ${PX4_SOURCE_DIR}/boards/atl/mantis-edu/upload.sh ${UPLOAD_NAME} + DEPENDS ${PX4_FW_NAME} + COMMENT "uploading autopilot.px4 file" + USES_TERMINAL +) diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board new file mode 100644 index 000000000000..4116386115f3 --- /dev/null +++ b/boards/atl/mantis-edu/default.px4board @@ -0,0 +1,53 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_BAROMETER_MAIERTEK_MPC2520=y +CONFIG_DRIVERS_TAP_ESC=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=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_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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_GIMBAL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SHUTDOWN=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/atl/mantis-edu/extras/atl_mantis-edu_bootloader.bin b/boards/atl/mantis-edu/extras/atl_mantis-edu_bootloader.bin new file mode 100755 index 000000000000..554ae898cecd Binary files /dev/null and b/boards/atl/mantis-edu/extras/atl_mantis-edu_bootloader.bin differ diff --git a/boards/atl/mantis-edu/firmware.prototype b/boards/atl/mantis-edu/firmware.prototype new file mode 100644 index 000000000000..5c37f437d787 --- /dev/null +++ b/boards/atl/mantis-edu/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 97, + "magic": "PX4FWv1", + "description": "Firmware for the Advanced Technology Labs (ATL) Mantis EDU (V18S) board", + "image": "", + "build_time": 0, + "summary": "atl_mantis-edu", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2064384, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/atl/mantis-edu/init/rc.board_defaults b/boards/atl/mantis-edu/init/rc.board_defaults new file mode 100644 index 000000000000..01c211c03e6f --- /dev/null +++ b/boards/atl/mantis-edu/init/rc.board_defaults @@ -0,0 +1,24 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default SYS_AUTOSTART 4061 + +param set-default BAT1_V_DIV 9.0 + +param set-default COM_ARM_SDCARD 0 + +param set-default SENS_EXT_I2C_PRB 0 + +param set-default EKF2_MULTI_IMU 1 +param set-default EKF2_MULTI_MAG 1 +param set-default SENS_IMU_MODE 0 +param set-default SENS_MAG_MODE 0 + +param set-default EV_TSK_STAT_DIS 1 + +param set-default SYS_DM_BACKEND 1 + +# Use MAVLink log streaming +set LOGGER_ARGS "-m mavlink -c 0.5" diff --git a/boards/atl/mantis-edu/init/rc.board_extras b/boards/atl/mantis-edu/init/rc.board_extras new file mode 100644 index 000000000000..e3969b7473d9 --- /dev/null +++ b/boards/atl/mantis-edu/init/rc.board_extras @@ -0,0 +1,5 @@ +#!/bin/sh + +tap_esc start -d /dev/ttyS4 -n 4 +sleep 1 +mixer load /dev/tap_esc /etc/mixers/quad_x.main.mix diff --git a/boards/atl/mantis-edu/init/rc.board_mavlink b/boards/atl/mantis-edu/init/rc.board_mavlink new file mode 100644 index 000000000000..c8bc7b224acb --- /dev/null +++ b/boards/atl/mantis-edu/init/rc.board_mavlink @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific MAVLink startup script. +#------------------------------------------------------------------------------ + +set GIMBAL_TTY /dev/ttyS3 +set MAV_RATE 40000 +set BAUDRATE 500000 + +mavlink start -r ${MAV_RATE} -d ${GIMBAL_TTY} -b ${BAUDRATE} +mavlink stream -d ${GIMBAL_TTY} -s SYSTEM_TIME -r 0.5 +mavlink stream -d ${GIMBAL_TTY} -s AUTOPILOT_STATE_FOR_GIMBAL_DEVICE -r 20 +mavlink stream -d ${GIMBAL_TTY} -s GIMBAL_DEVICE_SET_ATTITUDE -r 20 + +# optical flow +mavlink start -d /dev/ttyS2 -m custom -b 500000 diff --git a/boards/atl/mantis-edu/init/rc.board_sensors b/boards/atl/mantis-edu/init/rc.board_sensors new file mode 100644 index 000000000000..6c07fa135319 --- /dev/null +++ b/boards/atl/mantis-edu/init/rc.board_sensors @@ -0,0 +1,17 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# SPI1 icm20602 IMU +icm20602 start -s -b 1 -R 8 + +# I2C2 ist8310 magnetometer +ist8310 start -I -b 2 -R 14 + +# I2C4 mpc2520 barometer +mpc2520 start -I -b 4 + +gps start -d /dev/ttyS0 -p ubx diff --git a/boards/atl/mantis-edu/nuttx-config/include/board.h b/boards/atl/mantis-edu/nuttx-config/include/board.h new file mode 100644 index 000000000000..291c3e7e46e1 --- /dev/null +++ b/boards/atl/mantis-edu/nuttx-config/include/board.h @@ -0,0 +1,332 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v5/include/board.h + * + * Copyright (C) 2016-2018 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_V5_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V5_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-v5 board provides the following clock sources: + * + * X301: 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 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 2 <= PLLM <= 63 + * 192 <= PLLN <= 432 + * 192 MHz <= PLL_VCO <= 432MHz + * + * SYSCLK = PLL_VCO / PLLP + * Subject to + * + * PLLP = {2, 4, 6, 8} + * SYSCLK <= 216 MHz + * + * USB OTG FS, SDMMC and RNG Clock = PLL_VCO / PLLQ + * Subject to + * The USB OTG FS requires a 48 MHz clock to work correctly. The SDMMC + * and the random number generator need a frequency lower than or equal + * to 48 MHz to work correctly. + * + * 2 <= PLLQ <= 15 + */ + +/* Highest SYSCLK with USB OTG FS clock = 48 MHz + * + * PLL_VCO = (16,000,000 / 8) * 216 = 432 MHz + * SYSCLK = 432 MHz / 2 = 216 MHz + * USB OTG FS, SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz + */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(216) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(9) + +#define STM32_VCO_FREQUENCY ((STM32_HSE_FREQUENCY / 8) * 216) +#define STM32_SYSCLK_FREQUENCY (STM32_VCO_FREQUENCY / 2) +#define STM32_OTGFS_FREQUENCY (STM32_VCO_FREQUENCY / 9) + +/* Configure factors for PLLSAI clock */ +#define CONFIG_STM32F7_PLLSAI 1 +#define STM32_RCC_PLLSAICFGR_PLLSAIN RCC_PLLSAICFGR_PLLSAIN(192) +#define STM32_RCC_PLLSAICFGR_PLLSAIP RCC_PLLSAICFGR_PLLSAIP(8) +#define STM32_RCC_PLLSAICFGR_PLLSAIQ RCC_PLLSAICFGR_PLLSAIQ(4) +#define STM32_RCC_PLLSAICFGR_PLLSAIR RCC_PLLSAICFGR_PLLSAIR(2) + +/* Configure Dedicated Clock Configuration Register */ +#define STM32_RCC_DCKCFGR1_PLLI2SDIVQ RCC_DCKCFGR1_PLLI2SDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVQ RCC_DCKCFGR1_PLLSAIDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVR RCC_DCKCFGR1_PLLSAIDIVR(0) +#define STM32_RCC_DCKCFGR1_SAI1SRC RCC_DCKCFGR1_SAI1SEL(0) +#define STM32_RCC_DCKCFGR1_SAI2SRC RCC_DCKCFGR1_SAI2SEL(0) +#define STM32_RCC_DCKCFGR1_TIMPRESRC 0 +#define STM32_RCC_DCKCFGR1_DFSDM1SRC 0 +#define STM32_RCC_DCKCFGR1_ADFSDM1SRC 0 + + +/* Configure factors for PLLI2S clock */ +#define CONFIG_STM32F7_PLLI2S 1 +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SP RCC_PLLI2SCFGR_PLLI2SP(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) + +/* Configure Dedicated Clock Configuration Register 2 */ +#define STM32_RCC_DCKCFGR2_USART1SRC RCC_DCKCFGR2_USART1SEL_APB +#define STM32_RCC_DCKCFGR2_USART2SRC RCC_DCKCFGR2_USART2SEL_APB +#define STM32_RCC_DCKCFGR2_UART4SRC RCC_DCKCFGR2_UART4SEL_APB +#define STM32_RCC_DCKCFGR2_UART5SRC RCC_DCKCFGR2_UART5SEL_APB +#define STM32_RCC_DCKCFGR2_USART6SRC RCC_DCKCFGR2_USART6SEL_APB +#define STM32_RCC_DCKCFGR2_UART7SRC RCC_DCKCFGR2_UART7SEL_APB +#define STM32_RCC_DCKCFGR2_UART8SRC RCC_DCKCFGR2_UART8SEL_APB +#define STM32_RCC_DCKCFGR2_I2C1SRC RCC_DCKCFGR2_I2C1SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C2SRC RCC_DCKCFGR2_I2C2SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C3SRC RCC_DCKCFGR2_I2C3SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C4SRC RCC_DCKCFGR2_I2C4SEL_HSI +#define STM32_RCC_DCKCFGR2_LPTIM1SRC RCC_DCKCFGR2_LPTIM1SEL_APB +#define STM32_RCC_DCKCFGR2_CECSRC RCC_DCKCFGR2_CECSEL_HSI +#define STM32_RCC_DCKCFGR2_CK48MSRC RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_SDMMCSRC RCC_DCKCFGR2_SDMMCSEL_48MHZ +#define STM32_RCC_DCKCFGR2_SDMMC2SRC RCC_DCKCFGR2_SDMMC2SEL_48MHZ +#define STM32_RCC_DCKCFGR2_DSISRC RCC_DCKCFGR2_DSISEL_PHY + + +/* Several prescalers allow the configuration of the two AHB buses, the + * high-speed APB (APB2) and the low-speed APB (APB1) domains. The maximum + * frequency of the two AHB buses is 216 MHz while the maximum frequency of + * the high-speed APB domains is 108 MHz. The maximum allowed frequency of + * the low-speed APB domain is 54 MHz. + */ + +/* AHB clock (HCLK) is SYSCLK (216 MHz) */ +#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/4 (54 MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (108MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* SDMMC dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(118+2)=400 KHz + */ + +/* Use the Falling edge of the SDIO_CLK clock to change the edge the + * data and commands are change on + */ + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +#define STM32_SDMMC_INIT_CLKDIV (118 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(1+2)=16 MHz + * DMA OFF: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32F7_SDMMC_DMA +# define STM32_SDMMC_MMCXFR_CLKDIV (1 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(1+2)=16 MHz + * DMA OFF: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(2+2)=12 MHz + */ +//TODO #warning "Check Freq for 24mHz" + +#ifdef CONFIG_STM32F7_SDMMC_DMA +# define STM32_SDMMC_SDXFR_CLKDIV (1 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +/* FLASH wait states + * + * --------- ---------- ----------- + * VDD MAX SYSCLK WAIT STATES + * --------- ---------- ----------- + * 1.7-2.1 V 180 MHz 8 + * 2.1-2.4 V 216 MHz 9 + * 2.4-2.7 V 216 MHz 8 + * 2.7-3.6 V 216 MHz 7 + * --------- ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 7 + +/* 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 + +/* 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 /* PB7 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PB6 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 /* PD3 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 /* PD11 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_4 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_4 /* PD1 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_3 /* PB12 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PB13 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_2 /* PG9 */ +#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ +#define GPIO_USART6_RTS GPIO_USART6_RTS_2 /* PG8 */ +#define GPIO_USART6_CTS GPIO_USART6_CTS_2 /* PG15 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_2 /* PF6 */ +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ + +/* USART8: has no remap + * + * GPIO_UART8_RX PE0[CN12-64] + * GPIO_UART8_TX PE1[CN11-61] + */ + + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_3 /* PG11 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_1 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_1 /* PE2 */ + + +/* I2C */ +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_2 /* PF1 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_2 /* PF0 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ + +/* SDMMC1 + * + * VDD 3.3 + * GND + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * GPIO_SDMMC1_NCD PG0 + */ + +#endif /*__NUTTX_CONFIG_PX4_FMU_V5_INCLUDE_BOARD_H */ diff --git a/boards/atl/mantis-edu/nuttx-config/include/board_dma_map.h b/boards/atl/mantis-edu/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..2a017ed8c618 --- /dev/null +++ b/boards/atl/mantis-edu/nuttx-config/include/board_dma_map.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/* +| DMA1 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | SPDIFRX_DT | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | SPDIFRX_CS | SPI3_TX_2 | +| Channel 1 | I2C1_RX | I2C3_RX | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_TX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | - | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| Channel 8 | I2C3_TX_1 | I2C4_RX_1 | - | - | I2C2_TX_1 | - | I2C4_TX_1 | - | +| Channel 9 | - | SPI2_RX | - | - | - | - | SPI2_TX | - | +| | | | | | | | | | +| Usage | UART8_TX | USART3_RX | UART4_RX | USART3_TX_1 | | USART2_RX | UART8_RX | | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_2 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | SAI1_B_2 | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | SAI2_A | SPI1_TX_2 | SAI2_B | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDMMC1_1 | - | USART1_RX_2 | SDMMC1_2 | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| Channel 8 | DFSDM1_FLT0_1 | DFSDM1_FLT1_1 | DFSDM1_FLT2_1 | DFSDM1_FLT3_1 | DFSDM1_FLT0_2 | DFSDM1_FLT1_2 | DFSDM1_FLT2_2 | DFSDM1_FLT3_2 | +| Channel 9 | JPEG_IN_1 | JPEG_OUT | SPI4_TX_3 | JPEG_IN_2 | JPEG_OUT_2 | SPI5_RX_3 | - | - | +| Channel 10 | SAI1_B_3 | SAI2_B_1 | SAI2_A_1 | - | - | - | SAI1_A_3 | - | +| Channel 11 | SDMMC2_1 | - | QUADSPI_1 | - | - | SDMMC2_2 | - | - | +| | | | | | | | | | +| Usage | SPI1_RX_1 | | USART6_RX_2 | SPI1_TX_1 | | TIM1_UP | SDMMC1_2 | | + */ + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// DMAMAP_UART8_TX // DMA1, Stream 0, Channel 5 (PX4IO TX) +// DMAMAP_USART3_RX // DMA1, Stream 1, Channel 4 (TELEM2 RX) +// DMAMAP_UART4_RX // DMA1, Stream 2, Channel 4 (TELEM4 RX) +#define DMAMAP_USART3_TX DMAMAP_USART3_TX_1 // DMA1, Stream 3, Channel 4 (TELEM2 TX) +// DMAMAP_USART2_RX // DMA1, Stream 5, Channel 4 (TELEM1 RX) +// DMAMAP_UART8_RX // DMA1, Stream 6, Channel 5 (PX4IO RX) + + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMAMAP_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3 (SPI sensors RX) +// AVAILABLE // DMA2, Stream 1 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 // DMA2, Stream 2, Channel 5 +#define DMAMAP_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 (SPI sensors TX) +// AVAILABLE // DMA2, Stream 4 +// DMAMAP_TIM1_UP // DMA2, Stream 5, Channel 6 (DSHOT) +#define DMAMAP_SDMMC1 DMAMAP_SDMMC1_2 // DMA2, Stream 6, Channel 4 +// AVAILABLE // DMA2, Stream 7 diff --git a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..94f275a25bac --- /dev/null +++ b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig @@ -0,0 +1,238 @@ +# +# 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_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/atl/mantis-edu/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0061 +CONFIG_CDCACM_PRODUCTSTR="PX4 ATL Mantis-EDU" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="Advanced Technology Labs" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_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_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART5=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=3000 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/atl/mantis-edu/nuttx-config/scripts/script.ld b/boards/atl/mantis-edu/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..fc70ef1de834 --- /dev/null +++ b/boards/atl/mantis-edu/nuttx-config/scripts/script.ld @@ -0,0 +1,191 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 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. + * + ****************************************************************************/ + +/* The STM32F765IIT6 has 2048 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F765IIT6, 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 on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option byes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F765IIT6 will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F765IIT6 also has 512 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 128 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 368 KiB of SRAM1 beginning at address 0x2002:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2007:c000 + * + * 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. + * + * Bootloader reserves the first 32K bank (2 Mbytes Flash memory single bank) + * organization (256 bits read width) + * The next 2 32K sectors are reserved for parameters. + */ + +MEMORY +{ + FLASH_ITCM (rx) : ORIGIN = 0x00218000, LENGTH = 1952K + FLASH_AXIM (rx) : ORIGIN = 0x08018000, LENGTH = 1952K + + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K +} + +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(_main_toc) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xb4ecc2925d7d05c5) + . += 8; + *(.main_toc) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH_AXIM + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH_AXIM + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH_AXIM + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH_AXIM + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > SRAM1 AT > FLASH_AXIM + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > SRAM1 + + /* 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) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH_AXIM + + _framfuncs = LOADADDR(.ramfunc); + + /* Start of the image signature. This + * has to be in the end of the image + */ + .signature : { + _boot_signature = ALIGN(4); + } > FLASH_AXIM +} diff --git a/boards/atl/mantis-edu/src/CMakeLists.txt b/boards/atl/mantis-edu/src/CMakeLists.txt new file mode 100644 index 000000000000..398389f97d00 --- /dev/null +++ b/boards/atl/mantis-edu/src/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + pwr.cpp +) +add_dependencies(drivers_board arch_board_hw_info) + +target_link_libraries(drivers_board + PRIVATE + arch_board_hw_info + arch_spi + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h new file mode 100644 index 000000000000..2342446c131b --- /dev/null +++ b/boards/atl/mantis-edu/src/board_config.h @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU-v5 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_GREEN /* PC6 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) +#define GPIO_nLED_BLUE /* PC7 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +#define FLASH_BASED_PARAMS +#define RAM_BASED_MISSIONS + +// Hacks for MAVLink RC button input +#define ATL_MANTIS_RC_INPUT_HACKS + +// Hacks for MAVLink RC button input +#define ATL_MANTIS_RC_INPUT_HACKS + +/* + * 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) +#define ADC1_GPIO(n) GPIO_ADC1_IN##n + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ ADC1_GPIO(0), \ + /* PC0 */ ADC1_GPIO(10), \ + /* PC1 */ ADC1_GPIO(11), \ + /* PC2 */ ADC1_GPIO(12), \ + /* PC3 */ ADC1_GPIO(13), \ + /* PC4 */ ADC1_GPIO(14) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PA0 */ ADC1_CH(0) +#define ADC_BATTERY_CURRENT_CHANNEL 0 +#define ADC_SCALED_V5_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* PC1 */ ADC1_CH(11) +#define ADC_HW_VER_SENSE_CHANNEL /* PC2 */ ADC1_CH(12) +#define ADC_HW_REV_SENSE_CHANNEL /* PC3 */ ADC1_CH(13) +#define ADC1_SPARE_1_CHANNEL /* PC4 */ ADC1_CH(14) // POWER_AD + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL) | \ + (1 << ADC_HW_REV_SENSE_CHANNEL) | \ + (1 << ADC1_SPARE_1_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) + +/* HW Version and Revision drive signals Default to 1 to detect */ +#define BOARD_HAS_HW_VERSIONING + +#define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) +#define GPIO_HW_REV_SENSE /* PC3 */ ADC1_GPIO(13) +#define GPIO_HW_VER_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) +#define GPIO_HW_VER_SENSE /* PC2 */ ADC1_GPIO(12) +#define HW_INFO_INIT {'V','5','x', 'x',0} +#define HW_INFO_INIT_VER 2 +#define HW_INFO_INIT_REV 3 + +#define BOARD_TAP_ESC_MODE 2 // select closed-loop control mode for the esc +// #define BOARD_USE_ESC_CURRENT_REPORT + +// LED mapping +#define BOARD_FRONT_LED_MASK (1 << 0) | (1 << 3) +#define BOARD_REAR_LED_MASK (1 << 1) | (1 << 2) + +/* HEATER */ +#define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) + +#define BOARD_HAS_LED_PWM 1 +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 + +#define BOARD_UI_LED_PWM_DRIVE_ACTIVE_LOW 1 + +#define BOARD_ADC_BRICK_VALID 1 +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SD_CARD_EN /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7) +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS5" +#define RC_SERIAL_SINGLEWIRE + +/* power on/off */ +#define MS_PWR_BUTTON_DOWN 750 +#define KEY_AD_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTC|GPIO_PIN4) +#define POWER_ON_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5) +#define POWER_CHECK_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTF|GPIO_PIN0) + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* 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)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED + +/* FMUv5 never powers odd the Servo rail */ +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (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 BOARD_HAS_POWER_CONTROL 1 + +#define GPIO_CAM_PWR_ON_H /* PB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_CAM_PWR_ON_L /* PB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_HW_REV_DRIVE, \ + GPIO_HW_VER_DRIVE, \ + GPIO_HEATER_OUTPUT, \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_OTGFS_VBUS \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 1 + +__BEGIN_DECLS + +#ifndef __ASSEMBLY__ + +int stm32_sdio_initialize(void); + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control for the tap-v2 board. + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/atl/mantis-edu/src/i2c.cpp b/boards/atl/mantis-edu/src/i2c.cpp new file mode 100644 index 000000000000..781fbaf7312f --- /dev/null +++ b/boards/atl/mantis-edu/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(2), + initI2CBusInternal(4), +}; diff --git a/boards/atl/mantis-edu/src/init.c b/boards/atl/mantis-edu/src/init.c new file mode 100644 index 000000000000..461c16377e1a --- /dev/null +++ b/boards/atl/mantis-edu/src/init.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 init.c + * + * PX4FMU-specific early startup code. This file implements the + * board_app_initializ() 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. + */ + +/**************************************************************************** + * 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 +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +#define _GPIO_PULL_DOWN_INPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +/* 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 + +/***************cam_pwr_on_pulse***************************************************** + * Name: cam_pwr_on_pulse() + * + * Description: + * Camera power is controlled by flight control, send a risingedge pulse to enable it + * + * + * Input Parameters: + * Not used + * + ************************************************************************************/ +static void cam_pwr_on_pulse(void) +{ + static bool pwr_on_flag = false; + + if (pwr_on_flag == false) { + // This is now done in the bootloader. + // However, for transition we can leave it in. + up_mdelay(700); + stm32_configgpio(GPIO_CAM_PWR_ON_H); + up_mdelay(20); + stm32_configgpio(GPIO_CAM_PWR_ON_L); + + pwr_on_flag = true; + } +} +/************************************************************************************ + * 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) +{ +} + +/************************************************************************************ + * 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) +{ + /* Hold power state */ + board_pwr_init(0); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); +} + +/**************************************************************************** + * 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 */ + VDD_3V3_SD_CARD_EN(true); + + /* Need hrt running before using the ADC */ + px4_platform_init(); + + 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 SPI interfaces (after we determined the HW version) */ + stm32_spiinitialize(); + + /* 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 + + board_pwr_init(1); + + /* 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); + } + +#if defined(CONFIG_MMCSD) + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +#endif /* CONFIG_MMCSD */ + + /* Camera power is controlled by flight control, send a risingedge pulse to enable it */ + cam_pwr_on_pulse(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {1, 32 * 1024, 0x08008000}, + {2, 32 * 1024, 0x08010000}, + {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); + return result; + } + +#endif /* FLASH_BASED_PARAMS */ + + return OK; +} diff --git a/boards/atl/mantis-edu/src/led.c b/boards/atl/mantis-edu/src/led.c new file mode 100644 index 000000000000..bafff9ccdbd6 --- /dev/null +++ b/boards/atl/mantis-edu/src/led.c @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 + * + * PX4FMU 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_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + 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/atl/mantis-edu/src/pwr.cpp b/boards/atl/mantis-edu/src/pwr.cpp new file mode 100644 index 000000000000..e645d00529ed --- /dev/null +++ b/boards/atl/mantis-edu/src/pwr.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +/** + * @file pwr.c + * + * Board-specific power button functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include "board_config.h" +#include + +extern void led_on(int led); +extern void led_off(int led); + +static struct timespec time_down; + + +static int default_power_button_state_notification(board_power_button_state_notification_e request) +{ +// syslog(0,"%d\n", request); + return PWR_BUTTON_RESPONSE_SHUT_DOWN_NOW; +} + + +static power_button_state_notification_t power_state_notification = default_power_button_state_notification; + +/**************************************************************************** + * Name: board_pwr_button_down + * + * Description: + * Called to Read the logical state of the active low power button. + * + ****************************************************************************/ + +static bool board_pwr_button_down(void) +{ + return 0 == stm32_gpioread(KEY_AD_GPIO); +} + +int board_register_power_state_notification_cb(power_button_state_notification_t cb) +{ + power_state_notification = cb; + + if (board_pwr_button_down() && (time_down.tv_nsec != 0 || time_down.tv_sec != 0)) { + // make sure we don't miss the first event + power_state_notification(PWR_BUTTON_DOWN); + } + + return OK; +} + +int board_power_off(int status) +{ + led_on(BOARD_LED_BLUE); + + // disable the interrups + px4_enter_critical_section(); + + stm32_configgpio(POWER_OFF_GPIO); + + while (1); + + return 0; +} + +static int board_button_irq(int irq, FAR void *context, FAR void *args) +{ + if (board_pwr_button_down()) { + + led_on(BOARD_LED_RED); + clock_gettime(CLOCK_REALTIME, &time_down); + power_state_notification(PWR_BUTTON_DOWN); + + } else { + power_state_notification(PWR_BUTTON_UP); + + led_off(BOARD_LED_RED); + + struct timespec now; + clock_gettime(CLOCK_REALTIME, &now); + + uint64_t tdown_ms = time_down.tv_sec * 1000 + time_down.tv_nsec / 1000000; + + uint64_t tnow_ms = now.tv_sec * 1000 + now.tv_nsec / 1000000; + + if (tdown_ms != 0 && (tnow_ms - tdown_ms) >= MS_PWR_BUTTON_DOWN) { + + led_on(BOARD_LED_BLUE); + + if (power_state_notification(PWR_BUTTON_REQUEST_SHUT_DOWN) == PWR_BUTTON_RESPONSE_SHUT_DOWN_NOW) { + up_mdelay(200); + board_power_off(0); + } + + } else { + power_state_notification(PWR_BUTTON_IDEL); + } + + } + + return OK; +} + +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage) +{ + if (stage == 0) { + stm32_configgpio(POWER_ON_GPIO); + stm32_configgpio(KEY_AD_GPIO); + stm32_configgpio(POWER_CHECK_GPIO); + } + + if (stage == 1) { + stm32_gpiosetevent(KEY_AD_GPIO, true, true, true, board_button_irq, NULL); + } +} diff --git a/boards/atl/mantis-edu/src/sdio.c b/boards/atl/mantis-edu/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/atl/mantis-edu/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/atl/mantis-edu/src/spi.cpp b/boards/atl/mantis-edu/src/spi.cpp new file mode 100644 index 000000000000..5aceef3eafeb --- /dev/null +++ b/boards/atl/mantis-edu/src/spi.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/atl/mantis-edu/src/timer_config.cpp b/boards/atl/mantis-edu/src/timer_config.cpp new file mode 100644 index 000000000000..822773441936 --- /dev/null +++ b/boards/atl/mantis-edu/src/timer_config.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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 + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { +# if defined(BOARD_HAS_LED_PWM) && !defined(BOARD_HAS_CONTROL_STATUS_LEDS) + initIOTimer(Timer::Timer3), +# endif +}; + +/* Support driving active low (preferred) or active high LED + * on both the onboard status LEDs or the [n]UI_LED_[_EXTERNAL] + * + * Use open drain to drive the LED. This will ensure that + * if the LED has a 5 Volt supply that the LED will be + * off when high. + */ +#define CCER_C1_NUM_BITS 4 +#define ACTIVE_LOW(c) (GTIM_CCER_CC1P << (((c)-1) * CCER_C1_NUM_BITS)) +#define ACTIVE_HIGH(c) 0 + +#if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) +# define POLARITY(c) ACTIVE_LOW(c) +# define DRIVE_TYPE(p) ((p)|GPIO_OPENDRAIN) +#else +# define POLARITY(c) ACTIVE_HIGH((c)) +# define DRIVE_TYPE(p) (p) +#endif + +#if defined(BOARD_UI_LED_PWM_DRIVE_ACTIVE_LOW) +# define UI_POLARITY(c) ACTIVE_LOW(c) +# define UI_DRIVE_TYPE(p) ((p)|GPIO_OPENDRAIN) +#else +# define UI_POLARITY(c) ACTIVE_HIGH((c)) +# define UI_DRIVE_TYPE(p) (p) +#endif + +static inline constexpr timer_io_channels_t initIOTimerChannelUILED(const io_timers_t io_timers_conf[MAX_LED_TIMERS], + Timer::TimerChannel timer, GPIO::GPIOPin pin, int ui_polarity) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, timer, pin); + ret.gpio_out = UI_DRIVE_TYPE(ret.gpio_out); + ret.masks = UI_POLARITY(ui_polarity); + return ret; +} + +static inline constexpr timer_io_channels_t initIOTimerChannelControlLED(const io_timers_t + io_timers_conf[MAX_LED_TIMERS], Timer::TimerChannel timer, GPIO::GPIOPin pin, int polarity) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, timer, pin); + ret.gpio_out = DRIVE_TYPE(ret.gpio_out); + ret.masks = POLARITY(polarity); + return ret; +} + +constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +# if defined(BOARD_HAS_LED_PWM) && !defined(BOARD_HAS_CONTROL_STATUS_LEDS) + initIOTimerChannelControlLED(led_pwm_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}, 4), + initIOTimerChannelControlLED(led_pwm_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortC, GPIO::Pin6}, 1), + initIOTimerChannelControlLED(led_pwm_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortC, GPIO::Pin7}, 2), +# endif +}; diff --git a/boards/atl/mantis-edu/src/usb.c b/boards/atl/mantis-edu/src/usb.c new file mode 100644 index 000000000000..a10bf164956c --- /dev/null +++ b/boards/atl/mantis-edu/src/usb.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 +#include + +/************************************************************************************ + * 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/atl/mantis-edu/upload.sh b/boards/atl/mantis-edu/upload.sh new file mode 100755 index 000000000000..98e2a0686ae5 --- /dev/null +++ b/boards/atl/mantis-edu/upload.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash + +set -e + +PX4_BINARY_FILE="$1" + +echo "uploading: $PX4_BINARY_FILE" + +PX4_BINARY_FILE_SIZE=$(stat -c%s "$PX4_BINARY_FILE") +curl -v -F "image=@$PX4_BINARY_FILE" -H "Expect:" -H "File-Size:$PX4_BINARY_FILE_SIZE" http://192.168.42.1/cgi-bin/upload + diff --git a/boards/atlflight/cmake_hexagon b/boards/atlflight/cmake_hexagon deleted file mode 160000 index 08fd0a730453..000000000000 --- a/boards/atlflight/cmake_hexagon +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 08fd0a73045346448adf6969660196228b23e1fa diff --git a/boards/atlflight/eagle/CMakeLists.txt b/boards/atlflight/eagle/CMakeLists.txt deleted file mode 100644 index 2055ef2eaef7..000000000000 --- a/boards/atlflight/eagle/CMakeLists.txt +++ /dev/null @@ -1,32 +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. -# -############################################################################ diff --git a/boards/atlflight/eagle/cmake/upload.cmake b/boards/atlflight/eagle/cmake/upload.cmake deleted file mode 100644 index c50f88945d6d..000000000000 --- a/boards/atlflight/eagle/cmake/upload.cmake +++ /dev/null @@ -1,64 +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. -# -############################################################################ - -if("${PX4_PLATFORM}" MATCHES "qurt") - - add_custom_target(upload - COMMAND ${PX4_BOARD_DIR}/scripts/adb_upload.sh - ${PX4_BINARY_DIR}/platforms/qurt/libpx4.so ${PX4_BINARY_DIR}/platforms/qurt/libpx4muorb_skel.so ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/px4.config # source - /usr/share/data/adsp # destination - DEPENDS px4 px4muorb_skel ${PX4_BOARD_DIR}/scripts/adb_upload.sh - COMMENT "uploading px4" - USES_TERMINAL - ) - -else() - - add_custom_target(upload - COMMAND ${PX4_BOARD_DIR}/scripts/adb_upload.sh - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/eagle/flight/mainapp.config ${PX4_BINARY_DIR}/etc # source - /home/linaro # destination - DEPENDS px4 ${PX4_BOARD_DIR}/scripts/adb_upload.sh - COMMENT "uploading px4" - USES_TERMINAL - ) - - add_custom_target(sanity - COMMAND ./px4_snapflight_sanitytest.sh -i -t - DEPENDS px4 - WORKING_DIRECTORY ${PX4_BOARD_DIR}/scripts - COMMENT "uploading px4" - USES_TERMINAL - ) - -endif() diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake deleted file mode 100644 index 41295d615d31..000000000000 --- a/boards/atlflight/eagle/default.cmake +++ /dev/null @@ -1,129 +0,0 @@ - -# The Eagle board is the first generation Snapdragon Flight board by Qualcomm. - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon" - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon/toolchain" - ) - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# atlflight toolchain doesn't properly set the compiler, so these aren't set automatically -add_compile_options($<$:-std=gnu99>) -add_compile_options($<$:-std=gnu++11>) - -add_definitions( - -D__PX4_POSIX_EAGLE - -D__PX4_LINUX -) - -px4_add_board( - PLATFORM posix - VENDOR atlflight - MODEL eagle - LABEL default - #TESTING - TOOLCHAIN arm-linux-gnueabihf - ROMFSROOT px4fmu_common - DRIVERS - #barometer # all available barometer drivers - batt_smbus - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #lights/rgbled - #magnetometer # all available magnetometer drivers - pwm_out_sim - qshell/posix - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - #load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - muorb/krait - muorb/test - navigator - rc_update - rover_pos_control - sensors - #sih - temperature_compensation - simulator - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dumpfile - esc_calib - #hardfault_log - led_control - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - #tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - #fake_gps - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake deleted file mode 100644 index 1c9a1d3c6172..000000000000 --- a/boards/atlflight/eagle/qurt.cmake +++ /dev/null @@ -1,88 +0,0 @@ - -# The Eagle board is the first generation Snapdragon Flight board by Qualcomm. -# -# This cmake config builds for QURT which is the operating system running on -# the DSP side. - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - -if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "") - message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set") -else() - set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT}) -endif() - -include(toolchain/Toolchain-qurt) -include(qurt_flags) -include_directories(${HEXAGON_SDK_INCLUDES}) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -# This definition allows to differentiate the specific board. -add_definitions(-D__PX4_QURT_EAGLE) - -px4_add_board( - PLATFORM qurt - VENDOR atlflight - MODEL eagle - LABEL qurt - DRIVERS - barometer/bmp280 - gps - imu/invensense/mpu9250 - #magnetometer/hmc5883 - qshell/qurt - spektrum_rc - MODULES - airspeed_selector - attitude_estimator_q - commander - ekf2 - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - local_position_estimator - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - muorb/adsp - rc_update - rover_pos_control - sensors - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - led_control - mixer - #motor_ramp - motor_test - param - perf - #pwm - #topic_listener - ver - work_queue - ) diff --git a/boards/atlflight/eagle/scripts/.gitignore b/boards/atlflight/eagle/scripts/.gitignore deleted file mode 100644 index 65f4d26f13a5..000000000000 --- a/boards/atlflight/eagle/scripts/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -minidm.log -px4.log diff --git a/boards/atlflight/eagle/scripts/adb_upload.sh b/boards/atlflight/eagle/scripts/adb_upload.sh deleted file mode 100755 index 5b9475abcc14..000000000000 --- a/boards/atlflight/eagle/scripts/adb_upload.sh +++ /dev/null @@ -1,32 +0,0 @@ -#!/bin/bash - -if [[ "$#" < 2 ]]; then - echo "usage: adb_upload.sh SRC1 [SRC2 ...] DEST" - exit -fi - -# Get last argument -for last; do true; done - -echo "Wait for device..." -adb wait-for-device - -echo "Creating folder structure..." -adb shell mkdir -p $last - -echo "Uploading..." -# Go through source files and push them one by one. -i=0 -for arg -do - if [[ $((i+1)) == "$#" ]]; then - break - fi - # echo "Pushing $arg to $last" - adb push $arg $last - ((i+=1)) -done - -# Make sure they are synced to the file system -echo "Syncing FS..." -adb shell sync diff --git a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh deleted file mode 100755 index beed101566fa..000000000000 --- a/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh +++ /dev/null @@ -1,288 +0,0 @@ -#!/bin/bash - -################################################################################################# -# -# This script loads PX4 binaries to the Snapdragon Flight target and does a quick on-target sanity test. -# -# Pre-requisites: -# - Snapdragon Flight board connected to the host computer via USB cable -# - Snapdragon Flight board must have the latest platform BSP and flight controller addon installed" -# - mini-dm installed on host computer (see https://github.com/ATLFlight/ATLFlightDocs/blob/master/UserGuide.md#adsp) -# - PX4 software was built and binaries are in their usual locations in the Firmware tree. -# -# This script supports two modes: -# - Default mode (supported by PX4 community) -# - Legacy mode (uses proprietary drivers for ESC and RC Receiver, supported by Qualcomm) -# -# For help and cmd line options, run the script with the -h option -# -################################################################################################# - -# Halt on error -set -e - -# Verbose mode -## set -x - -# Mode of operation -readonly MODE_DEFAULT=0 -readonly MODE_LEGACY=1 -readonly MODE_8x96=2 -readonly MODE_MAX=$MODE_8x96 - -readonly RESULT_PASS=0 -readonly RESULT_FAIL=3 -readonly EXIT_ERROR=3 - - -# List of expected strings from the apps proc -declare -a appsproc_strings_present=( - "on udp port 14556 remote port 14550" - ) - -# List of unexpected strings from the apps proc -declare -a appsproc_strings_absent=( - "ERROR" - "Getting Bulk data from fastRPC link" - "Segmentation fault" - ) - -# List of expected strings from the DSP -declare -a dsp_strings_present=( - "EKF aligned" - ) - -# List of unexpected strings from the DSP -declare -a dsp_strings_absent=( - "Segmentation fault" - ) - - -install=0 -test=0 -mode=0 -result=$RESULT_PASS - -# Default mini-dm path (needs to be installed in this location or overriden through cmd line -minidmPath=~/Qualcomm/Hexagon_SDK/3.0/tools/debug/mini-dm/Linux_Debug - -# Default workspace path (parent directory of the script location) -workspace=`pwd`/../../../.. - - -verifypx4test() { - - #TODO: This needs to be fixed. For now, skip string checks for 8x96 platform. - if [ $mode == 2 ]; then - echo -e "[WARNING] Skipping string checks for 8x96 platform" - return - fi - - echo -e "Verifying test results..." - - # verify the presence of expected stings in the apps proc console log - for lineString in "${appsproc_strings_present[@]}" - do - if ! grep -Fq "$lineString" px4.log - then - # code if not found - echo -e "[ERROR] Missing expected string in apps proc log: $lineString" - result=$RESULT_FAIL - fi - done - - # verify the absence of unexpected stings in the apps proc console log - for lineString in "${appsproc_strings_absent[@]}" - do - if grep -Fq "$lineString" px4.log - then - # code if not found - echo -e "[ERROR] Found unexpected string in apps proc log: $lineString" - result=$RESULT_FAIL - fi - done - - echo -e "Displaying the content of the minidm.log" - cat minidm.log - echo -e "Analyzing the log for success and failure indications." - - # verify the presence of expected stings in the DSP console log - for lineString in "${dsp_strings_present[@]}" - do - if ! grep -Fq "$lineString" minidm.log - then - # code if not found - echo -e "[ERROR] Missing expected string in DSP log: $lineString" - result=$RESULT_FAIL - fi - done - - # verify the absence of unexpected stings in the DSP console log - for lineString in "${dsp_strings_absent[@]}" - do - if grep -Fq "$lineString" minidm.log - then - # code if not found - echo -e "[ERROR] Found unexpected string in DSP log: $lineString" - result=$RESULT_FAIL - fi - done - - echo -e "Verification complete." - - if [ $result -eq $RESULT_FAIL ]; then - echo -e "PX4 test result: FAIL" - else - echo -e "PX4 test result: PASS" - fi -} - -installpx4() { - - if [ $install -eq 0 ]; then - echo -e "SKIPPING install" - return 0; - fi - - # Reboot the target before beginning the installation - echo -e "Rebooting the target..." - adb reboot - adb wait-for-usb-device - # Wait a bit longer after bootup, before copying binaries to the target. - sleep 30 - adb devices - - echo -e "Now installing PX4 binaries..." - # Copy binaries to the target - if [ $mode == 0 ]; then - # copy default binaries - echo -e "Copying the PX4 binaries from the eagle_default build tree..." - adb push $workspace/build/atlflight_eagle_qurt/platforms/qurt/libpx4.so /usr/share/data/adsp - adb push $workspace/build/atlflight_eagle_qurt/platforms/qurt/libpx4muorb_skel.so /usr/share/data/adsp - adb push $workspace/build/atlflight_eagle_default/bin/px4 /home/linaro - adb push $workspace/posix-configs/eagle/flight/px4.config /usr/share/data/adsp - adb push $workspace/posix-configs/eagle/flight/mainapp.config /home/linaro - fi - - echo -e "Installation complete." -} - - -testpx4() { - - if [ $test -eq 0 ]; then - echo -e "SKIPPING test" - return 0; - fi - - echo -e "Starting PX4 test..." - - # Remove previous instances of the file - rm px4.log | true - rm minidm.log | true - - # Start mini-dm - echo -e "Starting mini-dm..." - ${minidmPath}/mini-dm > minidm.log & - sleep 5 - # Verify that mini-dm is running - checkProc=$(ps -aef | grep mini-dm | grep -v grep) - - if [ -z "${checkProc}" ]; then - echo "[ERROR] Unable to start mini-dm from path: ${minidmPath}" - exit $EXIT_ERROR - fi - - - # Start PX4 - echo -e "Starting PX4..." - if [ $mode == 2 ]; then - # 8x96 platform - adb shell "/home/root/px4 /home/root/mainapp.config" > px4.log 2>&1 & - else - # 8x74 platform - adb shell "/home/linaro/px4 /home/linaro/mainapp.config" > px4.log 2>&1 & - fi - sleep 20 - # Verify that PX4 is still running - checkProc=$(adb shell "ps -aef | grep px4 | grep -v grep") - if [ -z "${checkProc}" ]; then - echo "[ERROR] PX4 is not running on target!" - exit $EXIT_ERROR - fi - - # Stop the PX4 process on target - adb shell "ps -eaf | grep px4 | grep -v grep | awk '{print $2}' | tr -s ' ' | cut -d' ' -f2 | xargs kill" - sleep 5 - - # Stop the mini-dm - killall mini-dm - - echo -e "PX4 test complete." - - # Verify the results - verifypx4test - - echo -e "For more information, see px4.log and minidm.log." -} - - -usage() { - echo -e "\nThis script can copy PX4 binaries to the Snapdragon Flight target and do a quick on-target sanity test.\n" - echo -e "Pre-requisites:" - echo -e "- Snapdragon Flight board must be connected to host computer via USB" - echo -e "- Snapdragon Flight board must have the latest platform BSP and flight controller addon installed" - echo -e "- mini-dm must be installed on host computer (see https://github.com/ATLFlight/ATLFlightDocs/blob/master/UserGuide.md#adsp)" - echo -e "- PX4 software was built and binaries are in their usual locations in the tree\n" - echo -e "USAGE:\n ${0} [-m mode] [-i] [-t] [-l ]" - echo -e " -m --> Build mode (0 = default mode, 1 = legacy mode)" - echo -e " -i --> Install the PX4 binaries" - echo -e " -t --> Test PX4 on target" - echo -e " -l --> location of the mini-dm executable (Default: ${minidmPath})" - echo -e " -h --> Display this help information" -} - -# Parse the command line options -while getopts "m:l:ith" opt; - do - case $opt in - m) - if [ $OPTARG -gt $MODE_MAX ]; then - echo "Invalid mode: $OPTARG (max allowed is $MODE_MAX)" - exit $EXIT_ERROR - fi - mode=$OPTARG - echo "Will run the script in mode $mode." - ;; - i) - install=1 - ;; - t) - test=1 - ;; - l) - minidmPath=$OPTARG - ;; - h) - usage - exit 0 - ;; - :) - echo "Option -$OPTARG requires an argument" >&2 - exit 1;; - ?) - echo "Unknown arg $opt" - usage - exit 1 - ;; - esac -done - -# Install the PX4 binaries -installpx4 - -# Run the sanity test -testpx4 - -exit $result - diff --git a/boards/atlflight/eagle/src/CMakeLists.txt b/boards/atlflight/eagle/src/CMakeLists.txt deleted file mode 100644 index e2fb760a04b2..000000000000 --- a/boards/atlflight/eagle/src/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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(drivers_board - board_config.h - i2c.cpp - init.c - spi.cpp -) diff --git a/boards/atlflight/eagle/src/board_config.h b/boards/atlflight/eagle/src/board_config.h deleted file mode 100644 index b54dc0ae5430..000000000000 --- a/boards/atlflight/eagle/src/board_config.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 - * - * EAGLE internal definitions - */ - -#pragma once - -#define BOARD_OVERRIDE_UUID "EAGLEID000000000" // must be of length 16 -#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_EAGLE - -/* - * I2C busses - */ -#define PX4_NUMBER_I2C_BUSES 3 - -// Battery ADC channels - -#include -#include diff --git a/boards/atlflight/eagle/src/i2c.cpp b/boards/atlflight/eagle/src/i2c.cpp deleted file mode 100644 index 694c54188388..000000000000 --- a/boards/atlflight/eagle/src/i2c.cpp +++ /dev/null @@ -1,40 +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 - -constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusExternal(2), - initI2CBusExternal(3), - initI2CBusExternal(9), -}; diff --git a/boards/atlflight/eagle/src/spi.cpp b/boards/atlflight/eagle/src/spi.cpp deleted file mode 100644 index e740286c8a83..000000000000 --- a/boards/atlflight/eagle/src/spi.cpp +++ /dev/null @@ -1,41 +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 -#include - -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(1, { - initSPIDevice(DRV_IMU_DEVTYPE_MPU9250), - }), -}; diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake deleted file mode 100644 index 2f7ef433b968..000000000000 --- a/boards/atlflight/excelsior/default.cmake +++ /dev/null @@ -1,127 +0,0 @@ - -# Excelsior is the code name of a board currently in development. - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon" - "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon/toolchain" - ) - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# atlflight toolchain doesn't properly set the compiler, so these aren't set automatically -add_compile_options($<$:-std=gnu99>) -add_compile_options($<$:-std=gnu++11>) - -add_definitions( - -D__PX4_POSIX_EXCELSIOR - -D__PX4_LINUX -) - -px4_add_board( - PLATFORM posix - VENDOR atlflight - MODEL excelsior - LABEL default - #TESTING - TOOLCHAIN arm-oemllib32-linux-gnueabi - DRIVERS - #barometer # all available barometer drivers - batt_smbus - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #lights/rgbled - #magnetometer # all available magnetometer drivers - pwm_out_sim - qshell/posix - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - #load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - muorb/krait - muorb/test - navigator - rc_update - rover_pos_control - sensors - #sih - simulator - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dumpfile - esc_calib - #hardfault_log - led_control - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - #tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - #fake_gps - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake deleted file mode 100644 index 668b69414b6a..000000000000 --- a/boards/atlflight/excelsior/qurt.cmake +++ /dev/null @@ -1,88 +0,0 @@ - -# Excelsior is the code name of a board currently in development. -# -# This cmake config builds for QURT which is the operating system running on -# the DSP side. - -# Get $QC_SOC_TARGET from environment if existing. -if (DEFINED ENV{QC_SOC_TARGET}) - set(QC_SOC_TARGET $ENV{QC_SOC_TARGET}) -else() - set(QC_SOC_TARGET "APQ8074") -endif() - -include(px4_git) -px4_add_git_submodule(TARGET git_cmake_hexagon PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - -if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "") - message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set") -else() - set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT}) -endif() - -include(toolchain/Toolchain-qurt) -include(qurt_flags) -include_directories(${HEXAGON_SDK_INCLUDES}) - -set(CONFIG_SHMEM "1") -add_definitions(-DORB_COMMUNICATOR) - -# Disable the creation of the parameters.xml file by scanning individual -# source files, and scan all source files. This will create a parameters.xml -# file that contains all possible parameters, even if the associated module -# is not used. This is necessary for parameter synchronization between the -# ARM and DSP processors. -set(DISABLE_PARAMS_MODULE_SCOPING TRUE) - -# This definition allows to differentiate the specific board. -add_definitions(-D__PX4_QURT_EXCELSIOR) - -px4_add_board( - PLATFORM qurt - VENDOR atlflight - MODEL excelsior - LABEL qurt - DRIVERS - barometer/bmp280 - gps - imu/invensense/mpu9250 - magnetometer/hmc5883 - qshell/qurt - spektrum_rc - MODULES - airspeed_selector - attitude_estimator_q - commander - ekf2 - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - local_position_estimator - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - muorb/adsp - rc_update - rover_pos_control - sensors - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - led_control - mixer - #motor_ramp - motor_test - param - perf - #pwm - #topic_listener - ver - work_queue - ) diff --git a/boards/atlflight/excelsior/src/CMakeLists.txt b/boards/atlflight/excelsior/src/CMakeLists.txt deleted file mode 100644 index df3582f15aba..000000000000 --- a/boards/atlflight/excelsior/src/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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(drivers_board - board_config.h - init.c - spi.cpp - i2c.cpp -) diff --git a/boards/atlflight/excelsior/src/board_config.h b/boards/atlflight/excelsior/src/board_config.h deleted file mode 100644 index bf9426f3c7e0..000000000000 --- a/boards/atlflight/excelsior/src/board_config.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 - * - * EXCELSIOR internal definitions - */ - -#pragma once - -#define BOARD_OVERRIDE_UUID "EAGLEID000000000" // must be of length 16 -#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_EAGLE - -/* - * I2C busses - */ -#define PX4_NUMBER_I2C_BUSES 3 - -#include -#include diff --git a/boards/atlflight/excelsior/src/i2c.cpp b/boards/atlflight/excelsior/src/i2c.cpp deleted file mode 100644 index 694c54188388..000000000000 --- a/boards/atlflight/excelsior/src/i2c.cpp +++ /dev/null @@ -1,40 +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 - -constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusExternal(2), - initI2CBusExternal(3), - initI2CBusExternal(9), -}; diff --git a/boards/atlflight/excelsior/src/spi.cpp b/boards/atlflight/excelsior/src/spi.cpp deleted file mode 100644 index e740286c8a83..000000000000 --- a/boards/atlflight/excelsior/src/spi.cpp +++ /dev/null @@ -1,41 +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 -#include - -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(1, { - initSPIDevice(DRV_IMU_DEVTYPE_MPU9250), - }), -}; diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake deleted file mode 100644 index b41729227404..000000000000 --- a/boards/av/x-v1/default.cmake +++ /dev/null @@ -1,130 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR av - MODEL x-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - ETHERNET - SERIAL_PORTS - GPS1:/dev/ttyS6 - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - TEL3:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot - gps - #imu # all available imu drivers - imu/adis16477 - imu/adis16497 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - #tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mixer - motor_ramp - motor_test - nshterm - netman - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/av/x-v1/default.px4board b/boards/av/x-v1/default.px4board new file mode 100644 index 000000000000..dd6a04061cf7 --- /dev/null +++ b/boards/av/x-v1/default.px4board @@ -0,0 +1,103 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ADIS16477=y +CONFIG_DRIVERS_IMU_ADIS16497=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/av/x-v1/bootloader/av_x-v1_bootloader.bin b/boards/av/x-v1/extras/av_x-v1_bootloader.bin similarity index 100% rename from boards/av/x-v1/bootloader/av_x-v1_bootloader.bin rename to boards/av/x-v1/extras/av_x-v1_bootloader.bin diff --git a/boards/av/x-v1/init/rc.board_defaults b/boards/av/x-v1/init/rc.board_defaults index 661ad2481629..120085d418ad 100644 --- a/boards/av/x-v1/init/rc.board_defaults +++ b/boards/av/x-v1/init/rc.board_defaults @@ -5,3 +5,5 @@ # system_power unavailable param set-default CBRK_SUPPLY_CHK 894281 + +param set-default BAT1_V_DIV 10.13 diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 215d39b042c9..86caac97af20 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -11,7 +11,7 @@ lps22hb -s start lsm303agr -s -R 4 start -ms4525_airspeed -T 4515 -I -b 3 start +ms4515 -I -b 3 start if ! param greater SENS_EN_PMW3901 0 then diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index a978f73a1c47..f5dbb79fbe6e 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -37,7 +37,7 @@ # CONFIG_NSH_DISABLE_XD is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/av/x-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -51,6 +51,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -78,6 +79,7 @@ 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 @@ -91,7 +93,6 @@ CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -115,7 +116,6 @@ CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=y CONFIG_NET_SOLINGER=y CONFIG_NET_TCP=y CONFIG_NET_TCPBACKLOG=y @@ -123,16 +123,11 @@ CONFIG_NET_TCP_WRITE_BUFFERS=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -151,6 +146,8 @@ CONFIG_PTHREAD_STACK_MIN=512 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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -165,8 +162,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index 68a2022d9bf9..ed08d8ad005d 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -88,12 +88,6 @@ #define ADC_CHANNELS \ ((1 << ADC_BATTERY1_VOLTAGE_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (10.133333333f) -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -101,7 +95,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define DIRECT_INPUT_TIMER_CHANNELS 9 #define BOARD_CAPTURE_GPIO /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14) @@ -139,7 +132,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/av/x-v1/src/init.c b/boards/av/x-v1/src/init.c index 68fc740ed8df..8e12ee823e41 100644 --- a/boards/av/x-v1/src/init.c +++ b/boards/av/x-v1/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -160,22 +161,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif (void) board_hardfault_init(2, true); @@ -185,7 +175,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { syslog(LOG_ERR, "[boot] SDIO init failed\n"); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake deleted file mode 100644 index 7343cb3e2f40..000000000000 --- a/boards/beaglebone/blue/default.cmake +++ /dev/null @@ -1,108 +0,0 @@ -add_definitions( - -D__PX4_LINUX - - -DRC_AUTOPILOT_EXT # Enable extensions in Robotics Cape Library, TODO: remove -) - -px4_add_board( - VENDOR beaglebone - MODEL blue - LABEL default - PLATFORM posix - ARCHITECTURE cortex-a8 - ROMFSROOT px4fmu_common - TOOLCHAIN arm-linux-gnueabihf - TESTING - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/bmp280 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/mpu9250 - linux_pwm_out - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - pwm_out_sim - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - #simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - dyn - esc_calib - led_control - mixer - motor_ramp - motor_test - param - perf - pwm - sd_bench - #serial_test - system_time - shutdown - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board new file mode 100644 index 000000000000..db48c3e31823 --- /dev/null +++ b/boards/beaglebone/blue/default.px4board @@ -0,0 +1,86 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_LINUX=y +CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf" +CONFIG_BOARD_ARCHITECTURE="cortex-a8" +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_LINUX_PWM_OUT=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_FIXEDWING_CONTROL=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y +CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/beaglebone/blue/init/rc.board_defaults b/boards/beaglebone/blue/init/rc.board_defaults new file mode 100644 index 000000000000..d5f8c3bd4e0d --- /dev/null +++ b/boards/beaglebone/blue/init/rc.board_defaults @@ -0,0 +1,6 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 11.0 diff --git a/boards/beaglebone/blue/src/board_config.h b/boards/beaglebone/blue/src/board_config.h index 44de34ef38cb..b6c547916141 100644 --- a/boards/beaglebone/blue/src/board_config.h +++ b/boards/beaglebone/blue/src/board_config.h @@ -42,17 +42,20 @@ #define BOARD_OVERRIDE_UUID "BBBLUEID00000000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_BBBLUE -#define BOARD_BATTERY1_V_DIV (11.0f) - #define BOARD_MAX_LEDS 4 // Number external of LED's this board has // I2C +#define CONFIG_I2C 1 #define PX4_NUMBER_I2C_BUSES 2 #define PX4_I2C_OBDEV_MPU9250 0x68 +// SPI +#define CONFIG_SPI 1 + + // ADC channels: #define ADC_CHANNELS (1 << 5) #define BOARD_ADC_POS_REF_V (1.8f) diff --git a/boards/beaglebone/blue/src/spi.cpp b/boards/beaglebone/blue/src/spi.cpp index ce3841e169a2..78f5f02bae65 100644 --- a/boards/beaglebone/blue/src/spi.cpp +++ b/boards/beaglebone/blue/src/spi.cpp @@ -35,4 +35,8 @@ #include constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(1, { + initSPIDevice(DRV_DEVTYPE_UNUSED, 1), // spidev1.1 + initSPIDevice(DRV_DEVTYPE_UNUSED, 2), // spidev1.2 + }), }; diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake deleted file mode 100644 index 7547c13ee107..000000000000 --- a/boards/bitcraze/crazyflie/default.cmake +++ /dev/null @@ -1,72 +0,0 @@ -# workaround for syslink parameter PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7 -add_compile_options(-Wno-narrowing) - -px4_add_board( - PLATFORM nuttx - VENDOR bitcraze - MODEL crazyflie - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - CONSTRAINED_FLASH - DRIVERS - barometer/lps25h - distance_sensor/vl53l0x - gps - imu/invensense/mpu9250 - magnetometer/akm/ak8963 - optical_flow/pmw3901 - pwm_out - MODULES - attitude_estimator_q - #camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - sensors - #temperature_compensation - SYSTEMCMDS - #bl_update - dmesg - dumpfile - #esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board new file mode 100644 index 000000000000..41509df01688 --- /dev/null +++ b/boards/bitcraze/crazyflie/default.px4board @@ -0,0 +1,50 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing" +CONFIG_DRIVERS_BAROMETER_LPS25H=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y +CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_LAND_DETECTOR=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_MODULES_MC_ATT_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_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=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/bitcraze/crazyflie/bootloader/bitcraze_crazyflie_bootloader.bin b/boards/bitcraze/crazyflie/extras/bitcraze_crazyflie_bootloader.bin similarity index 100% rename from boards/bitcraze/crazyflie/bootloader/bitcraze_crazyflie_bootloader.bin rename to boards/bitcraze/crazyflie/extras/bitcraze_crazyflie_bootloader.bin diff --git a/boards/bitcraze/crazyflie/init/rc.board_mavlink b/boards/bitcraze/crazyflie/init/rc.board_mavlink deleted file mode 100644 index ae5089a30f4c..000000000000 --- a/boards/bitcraze/crazyflie/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Bitcraze Crazyflie specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig index 5995b5a0c80c..af6ca377c541 100644 --- a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig @@ -18,13 +18,14 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_SPI_CALLBACK is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/bitcraze/crazyflie/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -41,6 +42,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0016 CONFIG_CDCACM_PRODUCTSTR="PX4 Crazyflie v2.0" CONFIG_CDCACM_RXBUFSIZE=600 @@ -86,18 +88,13 @@ CONFIG_MTD=y CONFIG_MTD_AT24XX=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_FATDEVNO=0 CONFIG_NSH_LINELEN=128 @@ -129,8 +126,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/bitcraze/crazyflie/src/board_config.h b/boards/bitcraze/crazyflie/src/board_config.h index eb534441dd7c..5b9529a6823a 100644 --- a/boards/bitcraze/crazyflie/src/board_config.h +++ b/boards/bitcraze/crazyflie/src/board_config.h @@ -141,6 +141,7 @@ #define PX4_PWM_ALTERNATE_RANGES #define PWM_LOWEST_MIN 0 #define PWM_MOTOR_OFF 0 +#define PWM_SERVO_STOP 0 #define PWM_DEFAULT_MIN 20 #define PWM_HIGHEST_MIN 0 #define PWM_HIGHEST_MAX 255 @@ -153,7 +154,6 @@ #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) diff --git a/boards/bitcraze/crazyflie/src/init.c b/boards/bitcraze/crazyflie/src/init.c index 9ad2fb410bfd..c1364b75afe4 100644 --- a/boards/bitcraze/crazyflie/src/init.c +++ b/boards/bitcraze/crazyflie/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -130,23 +131,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) { px4_platform_init(); - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -161,12 +150,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #ifdef CONFIG_SPI - int ret = stm32_spi_bus_initialize(); - - if (ret != OK) { - return ret; - } - + stm32_spi_bus_initialize(); #endif /* Configure the HW based on the manifest */ diff --git a/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp b/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp index 92c9fcf753b1..04d31dd44d77 100644 --- a/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp +++ b/boards/bitcraze/crazyflie/syslink/ringbuffer.cpp @@ -244,22 +244,6 @@ RingBuffer::force(double val) return force(&val, sizeof(val)); } -// FIXME - clang crashes on this get() call -#ifdef __PX4_QURT -#define __PX4_SBCAP my_sync_bool_compare_and_swap -static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned b, unsigned c) -{ - if (*a == b) { - *a = c; - return true; - } - - return false; -} - -#else -#define __PX4_SBCAP __sync_bool_compare_and_swap -#endif bool RingBuffer::get(void *val, size_t val_size) { @@ -284,7 +268,7 @@ RingBuffer::get(void *val, size_t val_size) } /* if the tail pointer didn't change, we got our item */ - } while (!__PX4_SBCAP(&_tail, candidate, next)); + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); return true; diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index b6e7b06aaf54..d2faf8d2d9f6 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -53,7 +53,6 @@ #include #include -#include #include #include @@ -96,7 +95,7 @@ Syslink::Syslink() : _fd(0), _queue(2, sizeof(syslink_message_t)), _writebuffer(16, sizeof(crtp_message_t)), - _rssi(RC_INPUT_RSSI_MAX), + _rssi(input_rc_s::RSSI_MAX), _bstate(BAT_DISCHARGING) { px4_sem_init(&memory_sem, 0, 0); @@ -286,9 +285,6 @@ Syslink::task_main() _memory = new SyslinkMemory(this); _memory->init(); - _battery.reset(); - - // int ret; /* Open serial port */ @@ -408,9 +404,9 @@ Syslink::handle_message(syslink_message_t *msg) memcpy(&vbat, &msg->data[1], sizeof(float)); //memcpy(&iset, &msg->data[5], sizeof(float)); - _battery.updateBatteryStatus(t, vbat, -1, true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0, 0); - + _battery.setConnected(true); + _battery.updateVoltage(vbat); + _battery.updateAndPublishBatteryStatus(t); // Update battery charge state if (charging) { @@ -441,7 +437,7 @@ Syslink::handle_message(syslink_message_t *msg) px4_sem_post(&memory_sem); } else { - PX4_INFO("GOT %d", msg->type); + PX4_INFO("GOT %" PRIu8, msg->type); } //Send queued messages @@ -609,7 +605,7 @@ Syslink::handle_raw_other(syslink_message_t *sys) if (c->port == CRTP_PORT_LOG) { - PX4_INFO("Log: %d %d", c->channel, c->data[0]); + PX4_INFO("Log: %" PRIu8 " %" PRIu8, c->channel, c->data[0]); if (c->channel == 0) { // Table of Contents Access @@ -632,7 +628,7 @@ Syslink::handle_raw_other(syslink_message_t *sys) uint8_t cmd = c->data[0]; - PX4_INFO("Responding to cmd: %d", cmd); + PX4_INFO("Responding to cmd: %" PRIu8, cmd); c->data[2] = 0; // Success c->size = 3 + 1; @@ -673,7 +669,7 @@ Syslink::handle_raw_other(syslink_message_t *sys) } } else { - PX4_INFO("Got raw: %d", c->port); + PX4_INFO("Got raw: %" PRIu8, c->port); } } @@ -772,13 +768,13 @@ void status() printf("%i: ROM ID: ", i); for (int idi = 0; idi < idlen; idi++) { - printf("%02X", id[idi]); + printf("%02" PRIX8, id[idi]); } deck_descriptor_t desc; read(deckfd, &desc, sizeof(desc)); - printf(", VID: %02X , PID: %02X\n", desc.header, desc.vendorId, desc.productId); + printf("HDR:%02" PRIx8 ", VID: %02" PRIx8 " , PID: %02" PRIx8 "\n", desc.header, desc.vendorId, desc.productId); // Print pages of memory for (size_t di = 0; di < sizeof(desc); di++) { @@ -786,7 +782,7 @@ void status() printf("\n"); } - printf("%02X ", ((uint8_t *)&desc)[di]); + printf("%02" PRIX8 " ", ((uint8_t *)&desc)[di]); } diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index 875aa0347dd0..a54859508d3c 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -139,7 +139,7 @@ class Syslink // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; - Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US}; + Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE}; int32_t _rssi; battery_state _bstate; diff --git a/boards/bitcraze/crazyflie21/default.cmake b/boards/bitcraze/crazyflie21/default.cmake deleted file mode 100644 index bddc80c8259c..000000000000 --- a/boards/bitcraze/crazyflie21/default.cmake +++ /dev/null @@ -1,70 +0,0 @@ -# workaround for syslink parameter PARAM_DEFINE_INT32(SLNK_RADIO_ADDR2, 3890735079); // 0xE7E7E7E7 -add_compile_options(-Wno-narrowing) - -px4_add_board( - PLATFORM nuttx - VENDOR bitcraze - MODEL crazyflie21 - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - CONSTRAINED_FLASH - DRIVERS - barometer/bmp388 - distance_sensor/vl53l1x - gps - imu/bosch/bmi088/bmi088_i2c - optical_flow/pmw3901 - pwm_out - MODULES - attitude_estimator_q - #camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - #gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - sensors - #temperature_compensation - SYSTEMCMDS - #bl_update - dmesg - dumpfile - #esc_calib - hardfault_log - i2cdetect - led_control - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board new file mode 100644 index 000000000000..5f06c0fef957 --- /dev/null +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -0,0 +1,50 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing" +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088_I2C=y +CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=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_MODULES_MC_ATT_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_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=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/bitcraze/crazyflie21/bootloader/bitcraze_crazyflie21_bootloader.bin b/boards/bitcraze/crazyflie21/extras/bitcraze_crazyflie21_bootloader.bin similarity index 100% rename from boards/bitcraze/crazyflie21/bootloader/bitcraze_crazyflie21_bootloader.bin rename to boards/bitcraze/crazyflie21/extras/bitcraze_crazyflie21_bootloader.bin diff --git a/boards/bitcraze/crazyflie21/init/rc.board_mavlink b/boards/bitcraze/crazyflie21/init/rc.board_mavlink deleted file mode 100644 index ae5089a30f4c..000000000000 --- a/boards/bitcraze/crazyflie21/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Bitcraze Crazyflie specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig index 4bc2b274c872..72cd38e642f9 100644 --- a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig @@ -17,13 +17,14 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_SPI_CALLBACK is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/bitcraze/crazyflie21/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -40,6 +41,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0016 CONFIG_CDCACM_PRODUCTSTR="PX4 Crazyflie v2.0" CONFIG_CDCACM_RXBUFSIZE=300 @@ -84,9 +86,7 @@ CONFIG_MTD=y CONFIG_MTD_AT24XX=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y @@ -94,9 +94,6 @@ CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_EXPORT=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_FATDEVNO=0 CONFIG_NSH_LINELEN=128 @@ -128,8 +125,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/bitcraze/crazyflie21/src/board_config.h b/boards/bitcraze/crazyflie21/src/board_config.h index 72cf33cde07a..a8ee6029c2c0 100644 --- a/boards/bitcraze/crazyflie21/src/board_config.h +++ b/boards/bitcraze/crazyflie21/src/board_config.h @@ -142,6 +142,7 @@ #define PX4_PWM_ALTERNATE_RANGES #define PWM_LOWEST_MIN 0 #define PWM_MOTOR_OFF 0 +#define PWM_SERVO_STOP 0 #define PWM_DEFAULT_MIN 20 #define PWM_HIGHEST_MIN 0 #define PWM_HIGHEST_MAX 255 @@ -154,7 +155,6 @@ #define HRT_TIMER 8 /* use timer8 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) diff --git a/boards/bitcraze/crazyflie21/src/init.c b/boards/bitcraze/crazyflie21/src/init.c index 97e05c090ee3..6d1603600a36 100644 --- a/boards/bitcraze/crazyflie21/src/init.c +++ b/boards/bitcraze/crazyflie21/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -130,23 +131,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) { px4_platform_init(); - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -161,12 +150,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #ifdef CONFIG_SPI - int ret = stm32_spi_bus_initialize(); - - if (ret != OK) { - return ret; - } - + stm32_spi_bus_initialize(); #endif return OK; diff --git a/boards/cuav/can-gps-v1/canbootloader.cmake b/boards/cuav/can-gps-v1/canbootloader.cmake deleted file mode 100644 index 048aba388048..000000000000 --- a/boards/cuav/can-gps-v1/canbootloader.cmake +++ /dev/null @@ -1,13 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL can-gps-v1 - LABEL canbootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - DRIVERS - bootloaders -) diff --git a/boards/cuav/can-gps-v1/canbootloader.px4board b/boards/cuav/can-gps-v1/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/cuav/can-gps-v1/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/cuav/can-gps-v1/debug.cmake b/boards/cuav/can-gps-v1/debug.cmake deleted file mode 100644 index 90687e12f3fd..000000000000 --- a/boards/cuav/can-gps-v1/debug.cmake +++ /dev/null @@ -1,38 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -add_definitions(-DUSE_S_RGB_LED_DMA) - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL can-gps-v1 - LABEL debug - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 1 - DRIVERS - barometer/ms5611 - bootloaders - gps - lights/neopixel - magnetometer/rm3100 - safety_button - tone_alarm - uavcannode - MODULES - load_mon - SYSTEMCMDS - i2cdetect - led_control - param - perf - reboot - top - topic_listener - tune_control - uorb - ver - work_queue -) diff --git a/boards/cuav/can-gps-v1/default.cmake b/boards/cuav/can-gps-v1/default.cmake deleted file mode 100644 index 6f1eb8c488e0..000000000000 --- a/boards/cuav/can-gps-v1/default.cmake +++ /dev/null @@ -1,38 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -add_definitions(-DUSE_S_RGB_LED_DMA) - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL can-gps-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 1 - DRIVERS - barometer/ms5611 - bootloaders - gps - lights/neopixel - magnetometer/rm3100 - safety_button - tone_alarm - uavcannode - MODULES - #load_mon - SYSTEMCMDS - #i2cdetect - #led_control - param - #perf - #reboot - #top - #topic_listener - #tune_control - #uorb - #ver - #work_queue -) diff --git a/boards/cuav/can-gps-v1/default.px4board b/boards/cuav/can-gps-v1/default.px4board new file mode 100644 index 000000000000..3a838e1cbebd --- /dev/null +++ b/boards/cuav/can-gps-v1/default.px4board @@ -0,0 +1,29 @@ +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_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA" +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y +CONFIG_DRIVERS_MAGNETOMETER_RM3100=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=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/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig b/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig index 73eae9027f41..01f7bafa114b 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig @@ -7,7 +7,7 @@ # CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/can-gps-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -36,17 +36,10 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=0 +CONFIG_FS_PROCFS_MAX_TASKS=0 CONFIG_MM_REGIONS=2 CONFIG_NAME_MAX=0 CONFIG_NUNGET_CHARS=0 -CONFIG_NXFONTS_DISABLE_16BPP=y -CONFIG_NXFONTS_DISABLE_1BPP=y -CONFIG_NXFONTS_DISABLE_24BPP=y -CONFIG_NXFONTS_DISABLE_2BPP=y -CONFIG_NXFONTS_DISABLE_32BPP=y -CONFIG_NXFONTS_DISABLE_4BPP=y -CONFIG_NXFONTS_DISABLE_8BPP=y CONFIG_PREALLOC_TIMERS=0 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=262144 diff --git a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig index 9a8302a9b1ca..c33c19ca78e2 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig @@ -15,12 +15,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_DMACAPABLE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/can-gps-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -30,6 +31,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_BOARD_RESET_ON_ASSERT=2 @@ -70,18 +72,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -97,7 +94,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -112,8 +108,7 @@ CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y @@ -140,7 +135,6 @@ CONFIG_STM32_RTC_MAGIC_REG=1 CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef CONFIG_STM32_SERIALBRK_BSDCOMPAT=y CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_SPI1=y CONFIG_STM32_TIM8=y CONFIG_STM32_USART1=y diff --git a/boards/cuav/can-gps-v1/src/CMakeLists.txt b/boards/cuav/can-gps-v1/src/CMakeLists.txt index 7b84c71cc5fb..c85479c82054 100644 --- a/boards/cuav/can-gps-v1/src/CMakeLists.txt +++ b/boards/cuav/can-gps-v1/src/CMakeLists.txt @@ -61,6 +61,5 @@ else() nuttx_arch nuttx_drivers px4_layer - arch_io_pins ) endif() diff --git a/boards/cuav/can-gps-v1/src/init.c b/boards/cuav/can-gps-v1/src/init.c index 36f445546e8c..2466d25452de 100644 --- a/boards/cuav/can-gps-v1/src/init.c +++ b/boards/cuav/can-gps-v1/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -144,7 +145,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); - return -ENODEV; } #endif // FLASH_BASED_PARAMS diff --git a/boards/cuav/nora/bootloader.cmake b/boards/cuav/nora/bootloader.cmake deleted file mode 100644 index 3aef9b25990f..000000000000 --- a/boards/cuav/nora/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL nora - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/cuav/nora/bootloader.px4board b/boards/cuav/nora/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/cuav/nora/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake deleted file mode 100644 index f0b9361a168f..000000000000 --- a/boards/cuav/nora/default.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL nora - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - TESTING - UAVCAN_INTERFACES 2 - UAVCAN_TIMER_OVERRIDE 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - GPS2:/dev/ttyS2 - TEL2:/dev/ttyS3 - # CONSOLE: /dev/ttyS4 - # RC: /dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi088 - imu/invensense/icm20649 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board new file mode 100644 index 000000000000..1c7e0183987f --- /dev/null +++ b/boards/cuav/nora/default.px4board @@ -0,0 +1,106 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/cuav/nora/extras/cuav_nora_bootloader.bin b/boards/cuav/nora/extras/cuav_nora_bootloader.bin new file mode 100755 index 000000000000..fe1e749e7f57 Binary files /dev/null and b/boards/cuav/nora/extras/cuav_nora_bootloader.bin differ diff --git a/boards/cuav/nora/init/rc.board_defaults b/boards/cuav/nora/init/rc.board_defaults index 7491c4b0da8c..bbc9316544b3 100644 --- a/boards/cuav/nora/init/rc.board_defaults +++ b/boards/cuav/nora/init/rc.board_defaults @@ -3,16 +3,20 @@ # board specific defaults #------------------------------------------------------------------------------ -param set-default BAT_V_DIV 18 +# Enables the 2nd bank of mixers +set AUX_BANK2 yes + param set-default BAT1_V_DIV 18 param set-default BAT2_V_DIV 18 -param set-default BAT_A_PER_V 24 param set-default BAT1_A_PER_V 24 param set-default BAT2_A_PER_V 24 # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 +# Set Camera trigger pins to 13/14 +param set-default TRIG_PINS_EX 12288 + rgbled_pwm start safety_button start diff --git a/boards/cuav/nora/init/rc.board_sensors b/boards/cuav/nora/init/rc.board_sensors index 05375d625c32..b6dcb5b0cb6b 100644 --- a/boards/cuav/nora/init/rc.board_sensors +++ b/boards/cuav/nora/init/rc.board_sensors @@ -12,11 +12,17 @@ rm3100 -s -b 2 start # SPI4 bmi088 -s -b 4 -A -R 2 start -bmi088 -s -b 4 -G -R 2 start +if ! bmi088 -s -b 4 -G -R 2 start +then + icm42688p -R 2 -s start +fi ms5611 -s -b 4 start # SPI6 -icm20649 -s -b 6 -R 2 start +if ! icm20649 -s -b 6 -R 2 start +then + icm20689 -s -b 6 -R 2 start +fi ms5611 -s -b 6 start # External compass on GPS1/I2C1: standard CUAV GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/cuav/nora/nuttx-config/bootloader/defconfig b/boards/cuav/nora/nuttx-config/bootloader/defconfig index 8dfb24534c6e..9ea14fb7c97b 100644 --- a/boards/cuav/nora/nuttx-config/bootloader/defconfig +++ b/boards/cuav/nora/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/nora/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004c CONFIG_CDCACM_PRODUCTSTR="PX4 BL CUAV Nora" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,8 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART6_RXBUFSIZE=600 CONFIG_USART6_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/cuav/nora/nuttx-config/include/board.h b/boards/cuav/nora/nuttx-config/include/board.h index 6d78671f81a0..ad4b16fbd518 100644 --- a/boards/cuav/nora/nuttx-config/include/board.h +++ b/boards/cuav/nora/nuttx-config/include/board.h @@ -250,25 +250,27 @@ #define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ /* SPI */ -#define GPIO_SPI1_SCK GPIO_SPI1_SCK_3 /* PG11 */ -#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ -#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_6 /* PI1 */ -#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ -#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_3) /* PG11 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */ -#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ -#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ -#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_6) /* PI1 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ -#define GPIO_SPI5_SCK GPIO_SPI5_SCK_1 /* PF7 */ -#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ -#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_2) /* PE2 */ +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ -#define GPIO_SPI6_SCK GPIO_SPI6_SCK_1 /* PG13 */ -#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */ -#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_2 /* PA7 */ +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + +#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */ +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_2 /* PA7 */ /* I2C */ #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ diff --git a/boards/cuav/nora/nuttx-config/nsh/defconfig b/boards/cuav/nora/nuttx-config/nsh/defconfig index 0bfe98155400..8b0629c4d027 100644 --- a/boards/cuav/nora/nuttx-config/nsh/defconfig +++ b/boards/cuav/nora/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/nora/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004c CONFIG_CDCACM_PRODUCTSTR="PX4 CUAV Nora" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +98,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -116,13 +114,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -141,8 +139,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -191,7 +188,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM12=y CONFIG_STM32H7_TIM15=y CONFIG_STM32H7_TIM1=y @@ -213,7 +209,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/cuav/nora/nuttx-config/scripts/script.ld b/boards/cuav/nora/nuttx-config/scripts/script.ld index 025930da82dd..f213ad2ff429 100644 --- a/boards/cuav/nora/nuttx-config/scripts/script.ld +++ b/boards/cuav/nora/nuttx-config/scripts/script.ld @@ -111,6 +111,7 @@ MEMORY { ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + 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 */ @@ -186,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); diff --git a/boards/cuav/nora/src/CMakeLists.txt b/boards/cuav/nora/src/CMakeLists.txt index 24ea41f174f2..2c040010bd8b 100644 --- a/boards/cuav/nora/src/CMakeLists.txt +++ b/boards/cuav/nora/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/cuav/nora/src/board_config.h b/boards/cuav/nora/src/board_config.h index fcb2cbd4658e..56dbf9cc8c97 100644 --- a/boards/cuav/nora/src/board_config.h +++ b/boards/cuav/nora/src/board_config.h @@ -105,6 +105,7 @@ /* HEATER */ #define GPIO_HEATER_OUTPUT /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 14 @@ -117,8 +118,8 @@ #define GPIO_nPOWER_IN_CAN /* PG2 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2) #define GPIO_nPOWER_IN_C /* PG0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0) -#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */ -#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen */ +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen */ #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB is Chosen */ #define GPIO_VDD_5V_HIPOWER_EN /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) @@ -198,7 +199,6 @@ /* This board provides the board_on_reset interface */ #define BOARD_HAS_ON_RESET 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cuav/nora/src/bootloader_main.c b/boards/cuav/nora/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/cuav/nora/src/bootloader_main.c +++ b/boards/cuav/nora/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/cuav/nora/src/init.c b/boards/cuav/nora/src/init.c index c9f3b60086b3..d4c262b76a8b 100644 --- a/boards/cuav/nora/src/init.c +++ b/boards/cuav/nora/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -197,12 +201,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/cuav/nora/src/spi.cpp b/boards/cuav/nora/src/spi.cpp index 02dd0213dbf4..0aa6c5024819 100644 --- a/boards/cuav/nora/src/spi.cpp +++ b/boards/cuav/nora/src/spi.cpp @@ -46,6 +46,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI4, { initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortG, GPIO::Pin10}), }), initSPIBusExternal(SPI::Bus::SPI5, { @@ -55,6 +56,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }), initSPIBus(SPI::Bus::SPI6, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin12}, SPI::DRDY{GPIO::PortH, GPIO::Pin5}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortH, GPIO::Pin5}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortI, GPIO::Pin8}), }), }; diff --git a/boards/cuav/nora/src/usb.c b/boards/cuav/nora/src/usb.c index 360d84d6c846..3d86aca28017 100644 --- a/boards/cuav/nora/src/usb.c +++ b/boards/cuav/nora/src/usb.c @@ -41,6 +41,7 @@ #include #include #include +#include /************************************************************************************ * Name: stm32_usbsuspend diff --git a/boards/cuav/x7pro/bootloader.cmake b/boards/cuav/x7pro/bootloader.cmake deleted file mode 100644 index 93299343dc21..000000000000 --- a/boards/cuav/x7pro/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL x7pro - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/cuav/x7pro/bootloader.px4board b/boards/cuav/x7pro/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/cuav/x7pro/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake deleted file mode 100644 index 81266db7291e..000000000000 --- a/boards/cuav/x7pro/default.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cuav - MODEL x7pro - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - TESTING - UAVCAN_INTERFACES 2 - UAVCAN_TIMER_OVERRIDE 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - GPS2:/dev/ttyS2 - TEL2:/dev/ttyS3 - # CONSOLE: /dev/ttyS4 - # RC: /dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16470 - imu/bosch/bmi088 - imu/invensense/icm20649 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board new file mode 100644 index 000000000000..38d2db27ce32 --- /dev/null +++ b/boards/cuav/x7pro/default.px4board @@ -0,0 +1,109 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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 +CONFIG_EXAMPLES_FAKE_GPS=y +#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin b/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin new file mode 100755 index 000000000000..0b83b8cc0d13 Binary files /dev/null and b/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin differ diff --git a/boards/cuav/x7pro/init/rc.board_defaults b/boards/cuav/x7pro/init/rc.board_defaults index bc7bbe1ec2dc..1c199a4a72ed 100644 --- a/boards/cuav/x7pro/init/rc.board_defaults +++ b/boards/cuav/x7pro/init/rc.board_defaults @@ -6,18 +6,20 @@ # Enables the 2nd bank of mixers set AUX_BANK2 yes -param set-default BAT_V_DIV 18 param set-default BAT1_V_DIV 18 param set-default BAT2_V_DIV 18 -param set-default BAT_A_PER_V 24 param set-default BAT1_A_PER_V 24 param set-default BAT2_A_PER_V 24 # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 -param set-default IMU_GYRO_FFT_EN 1 +param set-default SENS_TEMP_ID 6946850 + +# Set Camera trigger pins to 13/14 +param set-default TRIG_PINS_EX 12288 + rgbled_pwm start safety_button start diff --git a/boards/cuav/x7pro/init/rc.board_mavlink b/boards/cuav/x7pro/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/cuav/x7pro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/cuav/x7pro/init/rc.board_sensors b/boards/cuav/x7pro/init/rc.board_sensors index e212013286e0..da9a45c93c0e 100644 --- a/boards/cuav/x7pro/init/rc.board_sensors +++ b/boards/cuav/x7pro/init/rc.board_sensors @@ -5,7 +5,7 @@ board_adc start # SPI1 -if ! icm20689 -s -b 1 -R 2 start +if ! icm20689 -s -b 1 -R 2 -q start then adis16470 -s -b 1 -R 2 start fi @@ -15,11 +15,17 @@ rm3100 -s -b 2 start # SPI4 bmi088 -s -b 4 -A -R 2 start -bmi088 -s -b 4 -G -R 2 start +if ! bmi088 -s -b 4 -G -R 2 start +then + icm42688p -R 2 -s start +fi ms5611 -s -b 4 start # SPI6 -icm20649 -s -b 6 -R 2 start +if ! icm20649 -s -b 6 -R 2 start +then + icm20689 -s -b 6 -R 2 start +fi ms5611 -s -b 6 start # External compass on GPS1/I2C1: standard CUAV GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig index f5d83bcba050..56d0a0be58d2 100644 --- a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig +++ b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/x7pro/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004c CONFIG_CDCACM_PRODUCTSTR="PX4 BL CUAV X7Pro" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,8 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART6_RXBUFSIZE=600 CONFIG_USART6_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/cuav/x7pro/nuttx-config/nsh/defconfig b/boards/cuav/x7pro/nuttx-config/nsh/defconfig index d513338736c5..277039146cf4 100644 --- a/boards/cuav/x7pro/nuttx-config/nsh/defconfig +++ b/boards/cuav/x7pro/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/x7pro/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,13 +46,13 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004c CONFIG_CDCACM_PRODUCTSTR="PX4 CUAV X7Pro" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x3163 CONFIG_CDCACM_VENDORSTR="CUAV" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -58,6 +60,8 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -70,6 +74,7 @@ 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 @@ -79,10 +84,8 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +98,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -116,13 +114,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -141,8 +139,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -191,7 +188,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM12=y CONFIG_STM32H7_TIM15=y CONFIG_STM32H7_TIM1=y @@ -213,7 +209,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/cuav/x7pro/nuttx-config/scripts/script.ld b/boards/cuav/x7pro/nuttx-config/scripts/script.ld index 025930da82dd..f213ad2ff429 100644 --- a/boards/cuav/x7pro/nuttx-config/scripts/script.ld +++ b/boards/cuav/x7pro/nuttx-config/scripts/script.ld @@ -111,6 +111,7 @@ MEMORY { ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + 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 */ @@ -186,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); diff --git a/boards/cuav/x7pro/nuttx-config/test/defconfig b/boards/cuav/x7pro/nuttx-config/test/defconfig new file mode 100644 index 000000000000..97b93866e505 --- /dev/null +++ b/boards/cuav/x7pro/nuttx-config/test/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_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cuav/x7pro/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743XI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004c +CONFIG_CDCACM_PRODUCTSTR="PX4 CUAV X7Pro" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3163 +CONFIG_CDCACM_VENDORSTR="CUAV" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y +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_LIBC_LONG_LONG=y +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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C3=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_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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_SPI6_DMA=y +CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_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_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +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_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_IFLOWCONTROL=y +CONFIG_USART6_OFLOWCONTROL=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/cuav/x7pro/src/CMakeLists.txt b/boards/cuav/x7pro/src/CMakeLists.txt index 24ea41f174f2..2c040010bd8b 100644 --- a/boards/cuav/x7pro/src/CMakeLists.txt +++ b/boards/cuav/x7pro/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/cuav/x7pro/src/board_config.h b/boards/cuav/x7pro/src/board_config.h index 2ffd6ef0eefe..f8a1824ebcd9 100644 --- a/boards/cuav/x7pro/src/board_config.h +++ b/boards/cuav/x7pro/src/board_config.h @@ -105,6 +105,7 @@ /* HEATER */ #define GPIO_HEATER_OUTPUT /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 14 @@ -117,8 +118,8 @@ #define GPIO_nPOWER_IN_CAN /* PG2 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2) #define GPIO_nPOWER_IN_C /* PG0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0) -#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */ -#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen */ +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen */ #define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB is Chosen */ #define GPIO_VDD_5V_HIPOWER_EN /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) @@ -198,7 +199,6 @@ /* This board provides the board_on_reset interface */ #define BOARD_HAS_ON_RESET 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cuav/x7pro/src/bootloader_main.c b/boards/cuav/x7pro/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/cuav/x7pro/src/bootloader_main.c +++ b/boards/cuav/x7pro/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/cuav/x7pro/src/init.c b/boards/cuav/x7pro/src/init.c index c9f3b60086b3..d4c262b76a8b 100644 --- a/boards/cuav/x7pro/src/init.c +++ b/boards/cuav/x7pro/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -197,12 +201,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/cuav/x7pro/src/spi.cpp b/boards/cuav/x7pro/src/spi.cpp index 5bef1b601272..671ad8d1bc6a 100644 --- a/boards/cuav/x7pro/src/spi.cpp +++ b/boards/cuav/x7pro/src/spi.cpp @@ -47,6 +47,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI4, { initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortG, GPIO::Pin10}), }), initSPIBusExternal(SPI::Bus::SPI5, { @@ -56,6 +57,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }), initSPIBus(SPI::Bus::SPI6, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin12}, SPI::DRDY{GPIO::PortH, GPIO::Pin5}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortH, GPIO::Pin5}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortI, GPIO::Pin8}), }), }; diff --git a/boards/cuav/x7pro/src/usb.c b/boards/cuav/x7pro/src/usb.c index 360d84d6c846..3d86aca28017 100644 --- a/boards/cuav/x7pro/src/usb.c +++ b/boards/cuav/x7pro/src/usb.c @@ -41,6 +41,7 @@ #include #include #include +#include /************************************************************************************ * Name: stm32_usbsuspend diff --git a/boards/cuav/x7pro/test.px4board b/boards/cuav/x7pro/test.px4board new file mode 100644 index 000000000000..ae1f94681188 --- /dev/null +++ b/boards/cuav/x7pro/test.px4board @@ -0,0 +1,13 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/cubepilot/cubeorange/bootloader.cmake b/boards/cubepilot/cubeorange/bootloader.cmake deleted file mode 100644 index 69782041e28b..000000000000 --- a/boards/cubepilot/cubeorange/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL cubeorange - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/cubepilot/cubeorange/bootloader.px4board b/boards/cubepilot/cubeorange/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/cubepilot/cubeorange/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake deleted file mode 100644 index 98afef8eedb2..000000000000 --- a/boards/cubepilot/cubeorange/console.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL cubeorange - LABEL console - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - IO cubepilot_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - # PX4IO:/dev/ttyS3 - # CONSOLE:/dev/ttyS4 - GPS2:/dev/ttyS5 - DRIVERS - #adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/invensense/icm20602 - imu/invensense/icm20649 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - #pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake deleted file mode 100644 index 95916fae13d1..000000000000 --- a/boards/cubepilot/cubeorange/default.cmake +++ /dev/null @@ -1,136 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL cubeorange - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - IO cubepilot_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - # PX4IO:/dev/ttyS3 - TEL3:/dev/ttyS4 - GPS2:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/icm20602 - imu/invensense/icm20649 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - #test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board new file mode 100644 index 000000000000..c8b1e35ed48a --- /dev/null +++ b/boards/cubepilot/cubeorange/default.px4board @@ -0,0 +1,103 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_IO="cubepilot_io-v2_default" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +#CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=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 +CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin new file mode 100755 index 000000000000..f31c6ae94794 Binary files /dev/null and b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin differ diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin new file mode 100755 index 000000000000..b91f26a198dc Binary files /dev/null and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeorange/init/rc.board_defaults b/boards/cubepilot/cubeorange/init/rc.board_defaults index d5e338224712..7f961cd33e71 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_defaults +++ b/boards/cubepilot/cubeorange/init/rc.board_defaults @@ -3,16 +3,15 @@ # board specific defaults #------------------------------------------------------------------------------ - -param set-default BAT_V_DIV 10.1 param set-default BAT1_V_DIV 10.1 param set-default BAT2_V_DIV 10.1 -param set-default BAT_A_PER_V 17 param set-default BAT1_A_PER_V 17 param set-default BAT2_A_PER_V 17 # Disable IMU thermal control param set-default SENS_EN_THERMAL 0 +param set-default -s SENS_TEMP_ID 2621474 + set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/cubepilot/cubeorange/init/rc.board_mavlink b/boards/cubepilot/cubeorange/init/rc.board_mavlink index 08fed10e71ec..1dd06663bdba 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_mavlink +++ b/boards/cubepilot/cubeorange/init/rc.board_mavlink @@ -3,5 +3,8 @@ # Board specific MAVLink startup script. #------------------------------------------------------------------------------ -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 +# Start ADS-B receiver mavlink connection if console not present +if [ ! -e /dev/console ] +then + mavlink start -d /dev/ttyS4 -b 57600 -m minimal +fi diff --git a/boards/cubepilot/cubeorange/init/rc.board_sensors b/boards/cubepilot/cubeorange/init/rc.board_sensors index 8535383b4b88..e84edf7e2ff6 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_sensors +++ b/boards/cubepilot/cubeorange/init/rc.board_sensors @@ -13,5 +13,3 @@ icm20948 -s -b 4 -R 10 -M start ms5611 -s -b 1 start icm20649 -s -b 1 start -# standard Here/Here2 connected to GPS1 -ak09916 -X -b 1 -R 6 start # external AK09916 (Here2) is rotated 270 degrees yaw diff --git a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig index 236fc2343538..0975c4e3726d 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -25,10 +25,11 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_INITTHREAD_PRIORITY=254 CONFIG_BOARD_LATE_INITIALIZE=y -CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_LOOPSPERMSEC=79954 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1016 CONFIG_CDCACM_PRODUCTSTR="PX4 BL CubePilot CubeOrange" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,8 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/console/defconfig b/boards/cubepilot/cubeorange/nuttx-config/console/defconfig deleted file mode 100644 index 3bd7d720c3f0..000000000000 --- a/boards/cubepilot/cubeorange/nuttx-config/console/defconfig +++ /dev/null @@ -1,235 +0,0 @@ -# -# 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_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_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743ZI=y -CONFIG_ARCH_CHIP_STM32H7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -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_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95150 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x1016 -CONFIG_CDCACM_PRODUCTSTR="CubeOrange" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x2DAE -CONFIG_CDCACM_VENDORSTR="CubePilot" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=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_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_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -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_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y -CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -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_BKPSRAM=y -CONFIG_STM32H7_DMA1=y -CONFIG_STM32H7_DMA2=y -CONFIG_STM32H7_DMACAPABLE=y -CONFIG_STM32H7_FLOWCONTROL_BROKEN=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_SPI1=y -CONFIG_STM32H7_SPI1_DMA=y -CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI4=y -CONFIG_STM32H7_SPI4_DMA=y -CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 -CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 -CONFIG_STM32H7_TIM1=y -CONFIG_STM32H7_TIM3=y -CONFIG_STM32H7_TIM4=y -CONFIG_STM32H7_UART4=y -CONFIG_STM32H7_UART7=y -CONFIG_STM32H7_UART8=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_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=1500 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=1500 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_TXBUFSIZE=3000 -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_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_WATCHDOG=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index fb7fc0c835b1..3a4abd0235e8 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -60,7 +60,6 @@ #define STM32_HSI_FREQUENCY 16000000ul #define STM32_LSI_FREQUENCY 32000 #define STM32_HSE_FREQUENCY STM32_BOARD_XTAL -#define STM32_LSE_FREQUENCY 0 /* Main PLL Configuration. * @@ -82,56 +81,55 @@ * PLLP2,3 = {2, 3, 4, ..., 128} * CPUCLK <= 480 MHz */ -#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE /* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR * - * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * PLL1_VCO = (24,000,000 / 3) * 100 = 800 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 + * PLL1P = PLL1_VCO/2 = 800 MHz / 2 = 400 MHz + * PLL1Q = PLL1_VCO/8 = 800 MHz / 8 = 100 MHz + * PLL1R = PLL1_VCO/2 = 800 MHz / 2 = 400 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(2) -#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(3) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(100) #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_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(8) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(2) -#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 3) * 100) #define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) -#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) -#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 8) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 2) /* 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(32) -#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_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(30) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(4) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(5) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(1) -#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) -#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) -#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) -#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 30) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 4) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 5) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 1) /* 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(32) -#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) * 32) -#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_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(6) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(72) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(3) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(6) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(9) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 6) * 72) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 3) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 6) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 9) + +/* SYSCLK = PLL1P = 400MHz + * CPUCLK = SYSCLK / 1 = 400 MHz */ #define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) #define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) @@ -140,7 +138,7 @@ /* Configure Clock Assignments */ /* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) - * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 200 */ #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 */ @@ -186,36 +184,28 @@ /* Kernel Clock Configuration * Note: look at Table 54 in ST Manual */ +#define STM32_RCC_D1CCIPR_SDMMCSEL RCC_D1CCIPR_SDMMC_PLL1 + #define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ #define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ #define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ -#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ -#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ -#define STM32_FDCANCLK STM32_HSE_FREQUENCY +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ /* FLASH wait states */ #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) +#define STM32_SDMMC_INIT_CLKDIV (125 << 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 +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 100 / (2*25) */ -#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_MMCXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#define STM32_SDMMC_SDXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) #define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE diff --git a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig index 07d469f5a0c1..997dd320d1fd 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -38,13 +39,15 @@ 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_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_LOOPSPERMSEC=79954 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1016 CONFIG_CDCACM_PRODUCTSTR="CubeOrange" CONFIG_CDCACM_RXBUFSIZE=600 @@ -71,6 +74,7 @@ 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 @@ -83,7 +87,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -96,18 +99,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,13 +115,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -142,8 +140,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -185,7 +182,6 @@ CONFIG_STM32H7_SPI4=y CONFIG_STM32H7_SPI4_DMA=y CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y @@ -204,7 +200,7 @@ CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/cubepilot/cubeorange/nuttx-config/scripts/script.ld b/boards/cubepilot/cubeorange/nuttx-config/scripts/script.ld index f408f6c2e965..f213ad2ff429 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/scripts/script.ld +++ b/boards/cubepilot/cubeorange/nuttx-config/scripts/script.ld @@ -187,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); diff --git a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig new file mode 100644 index 000000000000..21ae31492da3 --- /dev/null +++ b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig @@ -0,0 +1,234 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743ZI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=79954 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1016 +CONFIG_CDCACM_PRODUCTSTR="CubeOrange" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x2DAE +CONFIG_CDCACM_VENDORSTR="CubePilot" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y +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_LIBC_LONG_LONG=y +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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=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_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/cubepilot/cubeorange/src/CMakeLists.txt b/boards/cubepilot/cubeorange/src/CMakeLists.txt index 0c034edc6b1d..eacffd570277 100644 --- a/boards/cubepilot/cubeorange/src/CMakeLists.txt +++ b/boards/cubepilot/cubeorange/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/cubepilot/cubeorange/src/board_config.h b/boards/cubepilot/cubeorange/src/board_config.h index 0f31ae8ee24a..b5e19ea75224 100644 --- a/boards/cubepilot/cubeorange/src/board_config.h +++ b/boards/cubepilot/cubeorange/src/board_config.h @@ -62,7 +62,7 @@ #define GPIO_nLED_AMBER /* PE12 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 -#define BOARD_ARMED_STATE_LED LED_AMBER +#define BOARD_ARMED_LED LED_AMBER /* ADC channels */ #define PX4_ADC_GPIO \ @@ -115,6 +115,11 @@ #define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 #define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 +/* 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 + /* USB * OTG FS: PA9 OTG_FS_VBUS VBUS sensing */ @@ -143,9 +148,7 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cubepilot/cubeorange/src/bootloader_main.c b/boards/cubepilot/cubeorange/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/cubepilot/cubeorange/src/bootloader_main.c +++ b/boards/cubepilot/cubeorange/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/cubepilot/cubeorange/src/init.c b/boards/cubepilot/cubeorange/src/init.c index af6b2189f868..f7e1b3ca7793 100644 --- a/boards/cubepilot/cubeorange/src/init.c +++ b/boards/cubepilot/cubeorange/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -177,12 +181,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/cubepilot/cubeorange/src/usb.c b/boards/cubepilot/cubeorange/src/usb.c index 360d84d6c846..fae2e76255f4 100644 --- a/boards/cubepilot/cubeorange/src/usb.c +++ b/boards/cubepilot/cubeorange/src/usb.c @@ -41,6 +41,8 @@ #include #include #include +#include +#include /************************************************************************************ * Name: stm32_usbsuspend diff --git a/boards/cubepilot/cubeorange/test.px4board b/boards/cubepilot/cubeorange/test.px4board new file mode 100644 index 000000000000..e56c5bdd32aa --- /dev/null +++ b/boards/cubepilot/cubeorange/test.px4board @@ -0,0 +1,15 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake deleted file mode 100644 index adf01aeae2db..000000000000 --- a/boards/cubepilot/cubeyellow/console.cmake +++ /dev/null @@ -1,134 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL cubeyellow - LABEL console - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO cubepilot_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - # PX4IO:/dev/ttyS3 - # CONSOLE:/dev/ttyS4 - GPS2:/dev/ttyS5 - DRIVERS - #adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/invensense/icm20602 - imu/invensense/icm20649 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - #pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake deleted file mode 100644 index b0ef13753d53..000000000000 --- a/boards/cubepilot/cubeyellow/default.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL cubeyellow - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO cubepilot_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - # PX4IO:/dev/ttyS3 - TEL3:/dev/ttyS4 - GPS2:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/icm20602 - imu/invensense/icm20649 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - #test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board new file mode 100644 index 000000000000..4cd1d224881e --- /dev/null +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -0,0 +1,107 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_IO="cubepilot_io-v2_default" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin new file mode 100755 index 000000000000..b91f26a198dc Binary files /dev/null and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeyellow/init/rc.board_defaults b/boards/cubepilot/cubeyellow/init/rc.board_defaults index d5e338224712..6f39beb0c8ec 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_defaults +++ b/boards/cubepilot/cubeyellow/init/rc.board_defaults @@ -4,11 +4,9 @@ #------------------------------------------------------------------------------ -param set-default BAT_V_DIV 10.1 param set-default BAT1_V_DIV 10.1 param set-default BAT2_V_DIV 10.1 -param set-default BAT_A_PER_V 17 param set-default BAT1_A_PER_V 17 param set-default BAT2_A_PER_V 17 diff --git a/boards/cubepilot/cubeyellow/init/rc.board_mavlink b/boards/cubepilot/cubeyellow/init/rc.board_mavlink deleted file mode 100644 index 08fed10e71ec..000000000000 --- a/boards/cubepilot/cubeyellow/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/cubepilot/cubeyellow/init/rc.board_sensors b/boards/cubepilot/cubeyellow/init/rc.board_sensors index 8535383b4b88..e84edf7e2ff6 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_sensors +++ b/boards/cubepilot/cubeyellow/init/rc.board_sensors @@ -13,5 +13,3 @@ icm20948 -s -b 4 -R 10 -M start ms5611 -s -b 1 start icm20649 -s -b 1 start -# standard Here/Here2 connected to GPS1 -ak09916 -X -b 1 -R 6 start # external AK09916 (Here2) is rotated 270 degrees yaw diff --git a/boards/cubepilot/cubeyellow/nuttx-config/console/defconfig b/boards/cubepilot/cubeyellow/nuttx-config/console/defconfig deleted file mode 100644 index 5ced55a27892..000000000000 --- a/boards/cubepilot/cubeyellow/nuttx-config/console/defconfig +++ /dev/null @@ -1,238 +0,0 @@ -# -# 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_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32f7" -CONFIG_ARCH_CHIP_STM32F777VI=y -CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -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_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=22114 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x1012 -CONFIG_CDCACM_PRODUCTSTR="CubeYellow" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x2DAE -CONFIG_CDCACM_VENDORSTR="CubePilot" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=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_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_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y -CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_MODE=y -CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -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_STM32F7_ADC1=y -CONFIG_STM32F7_BBSRAM=y -CONFIG_STM32F7_BBSRAM_FILES=5 -CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_DMA1=y -CONFIG_STM32F7_DMA2=y -CONFIG_STM32F7_DMACAPABLE=y -CONFIG_STM32F7_FLOWCONTROL_BROKEN=y -CONFIG_STM32F7_I2C1=y -CONFIG_STM32F7_I2C2=y -CONFIG_STM32F7_I2C_DYNTIMEO=y -CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32F7_OTGFS=y -CONFIG_STM32F7_PROGMEM=y -CONFIG_STM32F7_PWR=y -CONFIG_STM32F7_RTC=y -CONFIG_STM32F7_RTC_HSECLOCK=y -CONFIG_STM32F7_RTC_MAGIC_REG=1 -CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC1=y -CONFIG_STM32F7_SDMMC_DMA=y -CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32F7_SPI1=y -CONFIG_STM32F7_SPI1_DMA=y -CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI2=y -CONFIG_STM32F7_SPI4=y -CONFIG_STM32F7_SPI4_DMA=y -CONFIG_STM32F7_SPI4_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI_DMA=y -CONFIG_STM32F7_SPI_DMATHRESHOLD=8 -CONFIG_STM32F7_TIM10=y -CONFIG_STM32F7_TIM11=y -CONFIG_STM32F7_UART4=y -CONFIG_STM32F7_UART7=y -CONFIG_STM32F7_UART8=y -CONFIG_STM32F7_USART2=y -CONFIG_STM32F7_USART3=y -CONFIG_STM32F7_USART6=y -CONFIG_STM32F7_USART_BREAKS=y -CONFIG_STM32F7_USART_INVERT=y -CONFIG_STM32F7_USART_SINGLEWIRE=y -CONFIG_STM32F7_USART_SWAP=y -CONFIG_STM32F7_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=1500 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_RXDMA=y -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1500 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=3000 -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_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig index 217c7ad3149d..4c1c3c5a57da 100644 --- a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig @@ -21,11 +21,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeyellow/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -39,6 +40,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -46,6 +48,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1012 CONFIG_CDCACM_PRODUCTSTR="CubeYellow" CONFIG_CDCACM_RXBUFSIZE=600 @@ -72,6 +75,7 @@ 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 @@ -84,7 +88,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -96,18 +99,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,16 +115,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -141,8 +141,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/cubepilot/cubeyellow/src/board_config.h b/boards/cubepilot/cubeyellow/src/board_config.h index 388b4f232b91..53fff3ac6ea6 100644 --- a/boards/cubepilot/cubeyellow/src/board_config.h +++ b/boards/cubepilot/cubeyellow/src/board_config.h @@ -62,7 +62,7 @@ #define GPIO_nLED_AMBER /* PE12 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 -#define BOARD_ARMED_STATE_LED LED_AMBER +#define BOARD_ARMED_LED LED_AMBER /* ADC channels */ #define PX4_ADC_GPIO \ @@ -143,9 +143,7 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/cubepilot/cubeyellow/src/init.c b/boards/cubepilot/cubeyellow/src/init.c index 3700662a175d..8ba89724027e 100644 --- a/boards/cubepilot/cubeyellow/src/init.c +++ b/boards/cubepilot/cubeyellow/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -163,22 +165,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #if defined(SERIAL_HAVE_RXDMA) - /* set up the serial DMA polling */ + // 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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* initial LED state */ @@ -196,12 +185,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/cubepilot/cubeyellow/src/usb.c b/boards/cubepilot/cubeyellow/src/usb.c index 360d84d6c846..343f3cbcada2 100644 --- a/boards/cubepilot/cubeyellow/src/usb.c +++ b/boards/cubepilot/cubeyellow/src/usb.c @@ -41,6 +41,9 @@ #include #include #include +#include +#include + /************************************************************************************ * Name: stm32_usbsuspend diff --git a/boards/cubepilot/io-v2/default.cmake b/boards/cubepilot/io-v2/default.cmake deleted file mode 100644 index 7be8f4d6cd5b..000000000000 --- a/boards/cubepilot/io-v2/default.cmake +++ /dev/null @@ -1,12 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR cubepilot - MODEL io-v2 - TOOLCHAIN arm-none-eabi - CONSTRAINED_FLASH - ARCHITECTURE cortex-m3 - DRIVERS - MODULES - px4iofirmware - ) diff --git a/boards/cubepilot/io-v2/default.px4board b/boards/cubepilot/io-v2/default.px4board new file mode 100644 index 000000000000..9e69a1e7460d --- /dev/null +++ b/boards/cubepilot/io-v2/default.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m3" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_MODULES_PX4IOFIRMWARE=y diff --git a/boards/cubepilot/io-v2/bootloader/cubepilot_io-v2_bootloader.bin b/boards/cubepilot/io-v2/extras/cubepilot_io-v2_bootloader.bin similarity index 100% rename from boards/cubepilot/io-v2/bootloader/cubepilot_io-v2_bootloader.bin rename to boards/cubepilot/io-v2/extras/cubepilot_io-v2_bootloader.bin diff --git a/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig b/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig index 97a82523b6a4..86125cabeea7 100644 --- a/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/io-v2/nuttx-config/nsh/defconfig @@ -8,13 +8,14 @@ # CONFIG_DEV_NULL is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/io-v2/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_STM32F100C8=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_C99_BOOL8=y CONFIG_DEBUG_FULLOPT=y @@ -27,12 +28,10 @@ CONFIG_FDCLONE_DISABLE=y CONFIG_FDCLONE_STDIO=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_IDLETHREAD_STACKSIZE=280 -CONFIG_MAX_TASKS=2 +CONFIG_IDLETHREAD_STACKSIZE=316 CONFIG_MM_FILL_ALLOCATIONS=y CONFIG_MM_SMALL=y CONFIG_NAME_MAX=12 -CONFIG_NFILE_DESCRIPTORS=3 CONFIG_PREALLOC_TIMERS=0 CONFIG_RAM_SIZE=8192 CONFIG_RAM_START=0x20000000 diff --git a/boards/cubepilot/io-v2/src/board_config.h b/boards/cubepilot/io-v2/src/board_config.h index 1a83e884451b..913564eb4ad3 100644 --- a/boards/cubepilot/io-v2/src/board_config.h +++ b/boards/cubepilot/io-v2/src/board_config.h @@ -47,8 +47,6 @@ #include #include -#include - /****************************************************************************** * Definitions ******************************************************************************/ @@ -72,29 +70,19 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) -#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) +#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY +#define GPIO_LED_GREEN (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING -#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true)) +#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true)) +#define LED_GREEN(on_true) stm32_gpiowrite(GPIO_LED_GREEN, (on_true)) -#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) -#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) -/* PixHawk 1: - * PC14 Floating - * PC15 Floating - * - * PixHawk 2: - * PC14 3.3v - * PC15 GND - */ +/* HEATER */ +#define GPIO_HEATER_OUTPUT /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define HEATER_OUTPUT_EN(on_true) stm32_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) -#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) -#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) -# define SENSE_PH1 0b10 /* Floating pulled as set */ -# define SENSE_PH2 0b01 /* Driven as tied */ #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) @@ -135,6 +123,7 @@ #define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) #define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define BOARD_HAS_NO_CAPTURE /* SBUS pins *************************************************************/ @@ -151,18 +140,6 @@ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8) -/* LED definitions ******************************************************************/ -/* PX4 has two LEDs that we will encode as: */ - -#define LED_STARTED 0 /* LED? */ -#define LED_HEAPALLOCATE 1 /* LED? */ -#define LED_IRQSENABLED 2 /* LED? + LED? */ -#define LED_STACKCREATED 3 /* LED? */ -#define LED_INIRQ 4 /* LED? + LED? */ -#define LED_SIGNAL 5 /* LED? + LED? */ -#define LED_ASSERTION 6 /* LED? + LED? + LED? */ -#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ - #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DISABLE_I2C_SPI +#include diff --git a/boards/cubepilot/io-v2/src/init.c b/boards/cubepilot/io-v2/src/init.c index 10c081d08b74..55ade132e18d 100644 --- a/boards/cubepilot/io-v2/src/init.c +++ b/boards/cubepilot/io-v2/src/init.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4iov2_init.c + * @file init.c * * PX4FMU-specific early startup code. This file implements the * stm32_boardinitialize() function that is called during cpu startup. @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -83,38 +84,14 @@ __EXPORT void stm32_boardinitialize(void) { - /* configure GPIOs */ - /* Set up for sensing HW */ - - stm32_configgpio(GPIO_SENSE_PC14_DN); - stm32_configgpio(GPIO_SENSE_PC15_UP); + stm32_configgpio(GPIO_HEATER_OUTPUT); /* LEDS - default to off */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_LED4); - - /* PixHawk 1: - * PC14 Floating - * PC15 Floating - * - * PixHawk 2: - * PC14 3.3v - * PC15 GND - */ - - uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN); - - if (sense == SENSE_PH2) { - stm32_configgpio(GPIO_HEATER_OFF); - } - - stm32_configgpio(GPIO_PC14); - stm32_configgpio(GPIO_PC15); - + stm32_configgpio(GPIO_LED_AMBER); + stm32_configgpio(GPIO_LED_SAFETY); + stm32_configgpio(GPIO_LED_GREEN); stm32_configgpio(GPIO_BTN_SAFETY); @@ -162,4 +139,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_PWM8, true); stm32_configgpio(GPIO_PWM8); + + /* disable heater */ + HEATER_OUTPUT_EN(false); } diff --git a/boards/cubepilot/io-v2/src/timer_config.cpp b/boards/cubepilot/io-v2/src/timer_config.cpp index f84e69921cb0..bb7dfa86fe53 100644 --- a/boards/cubepilot/io-v2/src/timer_config.cpp +++ b/boards/cubepilot/io-v2/src/timer_config.cpp @@ -35,8 +35,8 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer2), - initIOTimer(Timer::Timer3), initIOTimer(Timer::Timer4), + initIOTimer(Timer::Timer3), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { diff --git a/boards/omnibus/f4sd/bootloader/omnibus_f4sd_bootloader.bin b/boards/diatone/mamba-f405-mk2/bootloader/mamba_f405_mk2_bootloader.bin similarity index 100% rename from boards/omnibus/f4sd/bootloader/omnibus_f4sd_bootloader.bin rename to boards/diatone/mamba-f405-mk2/bootloader/mamba_f405_mk2_bootloader.bin diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board new file mode 100644 index 000000000000..447a60faaf94 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -0,0 +1,47 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_EXTERNAL_METADATA=y +CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_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_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_VER=y diff --git a/boards/diatone/mamba-f405-mk2/firmware.prototype b/boards/diatone/mamba-f405-mk2/firmware.prototype new file mode 100644 index 000000000000..cd7955b608ad --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 42, + "magic": "MAMBAF405MK2", + "description": "Firmware for the MambaF405Mk2 board", + "image": "", + "build_time": 0, + "summary": "PX4/MambaF405", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1032192, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/diatone/mamba-f405-mk2/init/rc.board_defaults b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults new file mode 100644 index 000000000000..97bbcd6ee691 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults @@ -0,0 +1,18 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# system_power unavailable +param set-default CBRK_SUPPLY_CHK 894281 + +# Disable safety switch by default +param set-default CBRK_IO_SAFETY 22027 + +# use the Q attitude estimator, it works w/o mag or GPS. +param set-default SYS_MC_EST_GROUP 3 +param set-default ATT_ACC_COMP 0 +param set-default ATT_W_ACC 0.4000 +param set-default ATT_W_GYRO_BIAS 0.0000 + +param set-default SYS_HAS_MAG 0 diff --git a/boards/diatone/mamba-f405-mk2/init/rc.board_extras b/boards/diatone/mamba-f405-mk2/init/rc.board_extras new file mode 100644 index 000000000000..d31babde7565 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/init/rc.board_extras @@ -0,0 +1,9 @@ +#!/bin/sh +# +# Mamba F405 MK2 specific board extras init +#------------------------------------------------------------------------------ + +if ! param compare OSD_ATXXXX_CFG 0 +then + atxxxx start -s +fi diff --git a/boards/diatone/mamba-f405-mk2/init/rc.board_sensors b/boards/diatone/mamba-f405-mk2/init/rc.board_sensors new file mode 100644 index 000000000000..2ce4db663ae9 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/init/rc.board_sensors @@ -0,0 +1,25 @@ +#!/bin/sh +# +# Mamba F405 MK2 specific board sensors init +#------------------------------------------------------------------------------ + +board_adc start + +if ! mpu6000 -R 6 -s start +then + # some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602 + icm20602 -s -R 6 start +fi + +if ! hmc5883 -T -X start +then + if mpu9250_i2c -X -b 2 -a 0x68 -R 28 start; then + sleep 1 # wait for mpu9250 to be configured with bypass enabled + ak8963 -X -b 2 -R 30 start # -R 34 + sleep 1 + mpu9250_i2c stop + ak8963 -X -b 2 -R 30 start # -R 34 + fi +fi + +bmp280 -X -b 2 start diff --git a/boards/diatone/mamba-f405-mk2/nuttx-config/include/board.h b/boards/diatone/mamba-f405-mk2/nuttx-config/include/board.h new file mode 100644 index 000000000000..041d55260824 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/include/board.h @@ -0,0 +1,327 @@ +/************************************************************************************ + * nuttx-configs/diatone_mamba-f405-mk2/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Author: Nathan Tsoi + * + * 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 __CONFIG_MAMBAF405MK2_INCLUDE_BOARD_H +#define __CONFIG_MAMBAF405MK2_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdio.h" +#include "stm32.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The mambaf405mk2 board features a single 8MHz crystal. Space is provided + * for a 32kHz RTC backup crystal, but it is not stuffed. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - 32.768 kHz + */ + +#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 + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#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/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8-11 are on APB2, others on APB1 + */ + +#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN +#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN +#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN +#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN +#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN +#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN +#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN +#define BOARD_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN +#define BOARD_TIM9_FREQUENCY STM32_APB2_TIM9_CLKIN +#define BOARD_TIM10_FREQUENCY STM32_APB2_TIM10_CLKIN +#define BOARD_TIM11_FREQUENCY STM32_APB2_TIM11_CLKIN +#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN +#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN +#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN + +/* SDIO dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(118+2)=400 KHz + */ + +#define SDIO_INIT_CLKDIV (118 << SDIO_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(1+2)=16 MHz + * DMA OFF: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32_SDIO_DMA +# define SDIO_MMCXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(1+2)=16 MHz + * DMA OFF: SDIOCLK=48MHz, SDIO_CK=SDIOCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32_SDIO_DMA +# define SDIO_SDXFR_CLKDIV (1 << SDIO_CLKCR_CLKDIV_SHIFT) +#else +# define SDIO_SDXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +#endif + +/* LED definitions ******************************************************************/ +/* 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 stm32_setled() */ + +#define BOARD_LED1 0 +//#define BOARD_LED2 1 +#define BOARD_NLEDS 1 + +#define BOARD_LED_BLUE BOARD_LED1 +//#define BOARD_LED_RED BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * omnibusf4sd. The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* UART1: + * + * PA10 (RX) and PA9 (TX) are broken out on J5 + */ + +#define GPIO_USART1_RX GPIO_USART1_RX_1 +#define GPIO_USART1_TX GPIO_USART1_TX_1 + +/* USART3: + * + * PC10 (TX) and PC11 (RX) are broken out on J4 + * + * However, this port is shared with SPI3 which contains the BMP280 and MAX7456 + * + * The Silkscreen pin labeled SCL is TX + * MISO is RX + */ +//#define GPIO_USART3_RX GPIO_USART3_RX_2 +//#define GPIO_USART3_TX GPIO_USART3_TX_2 + +/* UART4: + * + * PA0 (TX) -- Labeled RSSI on the silkscreen is only broken out on a test pad + * on the pro version. It's on a 2.54mm header on other versions + * PA1 (RX) -- Motor 5 out + */ +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +/* UART6: + * + * PC6 (TX) and PC7 (RX) are broken out on J10 + */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +/* SPI1: + * MPU6000 + * CS: PA4 -- configured in board_config.h + * CLK: PA5 + * MISO: PA6 + * MOSI: PA7 + */ + +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 + +/* SPI2: + * SD Card + * CS: PB12 -- configured in board_config.h + * CLK: PB13 + * MISO: PB14 + * MOSI: PB15 + */ + +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 + +/* SPI3: + * BMP280 + * CS: PB3 -- configured in board_config.h + * CLK: PC10 + * MISO: PC11 + * MOSI: PC12 + */ + +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_2 + +/* + * I2C (external) + * + * SCL: PB10 + * SDA: PB11 + * + * TODO: + * 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_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 + +#endif /* __CONFIG_OMNIBUSF4SD_INCLUDE_BOARD_H */ diff --git a/boards/diatone/mamba-f405-mk2/nuttx-config/include/board_dma_map.h b/boards/diatone/mamba-f405-mk2/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..d54cbdfecb4d --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/include/board_dma_map.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/* +| DMA1 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | - | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | - | SPI3_TX_2 | +| Channel 1 | I2C1_RX | - | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_RX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | I2C2_EXT_RX | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| | | | | | | | | | +| Usage | | TIM2_UP_1 | TIM3_UP | SPI2_RX | SPI2_TX | | | | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_1 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | - | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | - | SPI1_TX_2 | - | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDIO | - | USART1_RX_2 | SDIO | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| | | | | | | | | | +| Usage | SPI1_RX_1 | USART6_RX_1 | USART1_RX_1 | SPI1_TX_1 | | | SDIO | | + */ + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// AVAILABLE // DMA2, Stream 0 +// DMAMAP_TIM2_UP // DMA1, Stream 1, Channel 3 (DSHOT) +// DMAMAP_TIM3_UP // DMA1, Stream 2, Channel 5 (DSHOT) +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 (SPI2 RX) +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 (SPI2 TX) +// AVAILABLE // DMA2, Stream 5 +// AVAILABLE // DMA2, Stream 6 +// AVAILABLE // DMA2, Stream 7 + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3 (SPI1 RX) +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 // DMA2, Stream 1, Channel 4 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 (SPI1 TX) +// AVAILABLE // DMA2, Stream 4 +// AVAILABLE // DMA2, Stream 5 +#define DMAMAP_SDIO DMAMAP_SDIO_2 // DMA2, Stream 6, Channel 4 +// AVAILABLE // DMA2, Stream 7 diff --git a/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..7290e1d6b04b --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig @@ -0,0 +1,192 @@ +# +# 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_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_SPI_CALLBACK is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/diatone/mamba-f405-mk2/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_STM32F405RG=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0001 +CONFIG_CDCACM_PRODUCTSTR="DiatoneMambaF405 MK2" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="Diatone" +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_W25=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_BASENAME=y +CONFIG_NSH_DISABLE_DD=y +CONFIG_NSH_DISABLE_DIRNAME=y +CONFIG_NSH_DISABLE_HEXDUMP=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_LOSETUP=y +CONFIG_NSH_DISABLE_MKFIFO=y +CONFIG_NSH_DISABLE_MKRD=y +CONFIG_NSH_DISABLE_PUT=y +CONFIG_NSH_DISABLE_REBOOT=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_DISABLE_UNAME=y +CONFIG_NSH_DISABLE_WGET=y +CONFIG_NSH_DISABLE_XD=y +CONFIG_NSH_FATDEVNO=0 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=2 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=196608 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=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_I2C2=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=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=512 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=8 +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_UART4=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=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=600 +CONFIG_USART1_RXBUFSIZE=300 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/diatone/mamba-f405-mk2/nuttx-config/scripts/script.ld b/boards/diatone/mamba-f405-mk2/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c1d3ec63cf05 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/scripts/script.ld @@ -0,0 +1,138 @@ +/**************************************************************************** + * configs/omnibus-f4sd/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 STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb 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 CCM 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 16 KiB of flash is reserved for the bootloader. + * Paramater storage will use the next 16KiB Sector. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08008000, LENGTH = 992K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +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/diatone/mamba-f405-mk2/src/CMakeLists.txt b/boards/diatone/mamba-f405-mk2/src/CMakeLists.txt new file mode 100644 index 000000000000..4212b7e6d27b --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c +) + +target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/diatone/mamba-f405-mk2/src/board_config.h b/boards/diatone/mamba-f405-mk2/src/board_config.h new file mode 100644 index 000000000000..7c2b17d84db4 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/board_config.h @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2018, 2014 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 + * + * boards internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* omnibusf4sd GPIOs ***********************************************************************************/ +/* LEDs */ +// power - green +// LED1 - PB5 - blue +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_LED_BLUE GPIO_LED1 + +#define BOARD_OVERLOAD_LED LED_BLUE + +#define FLASH_BASED_PARAMS + +/* + * 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 + */ +#define ADC_CHANNELS (1 << 0) | (1 << 11) | (1 << 12) + +#define ADC_BATTERY_VOLTAGE_CHANNEL 11 +#define ADC_BATTERY_CURRENT_CHANNEL 13 +#define ADC_RC_RSSI_CHANNEL 12 + +/* Define Battery 1 Voltage Divider and A per V + */ +#define BOARD_BATTERY1_V_DIV (11.12f) +#define BOARD_BATTERY1_A_PER_V (31.f) + +/* User GPIOs + * + * GPIO0-5 are the PWM servo outputs. + * GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_1 //PB0 S1_OUT D1_ST7 + * GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_1 //PB1 S2_OUT D1_ST2 + * GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 //PA3 S3_OUT D1_ST6 + * GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 //PA2 S4_OUT D1_ST1 + * GPIO_TIM5_CH2OUT GPIO_TIM5_CH2OUT_1 //PA1 S5_OUT + * GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1 //PA8 S6_OUT + */ + +#define _MK_GPIO_INPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP)) + +#define GPIO_GPIO0_INPUT _MK_GPIO_INPUT(GPIO_TIM3_CH3IN) +#define GPIO_GPIO1_INPUT _MK_GPIO_INPUT(GPIO_TIM3_CH4IN) +#define GPIO_GPIO2_INPUT _MK_GPIO_INPUT(GPIO_TIM2_CH3IN) +#define GPIO_GPIO3_INPUT _MK_GPIO_INPUT(GPIO_TIM2_CH4IN) +//#define GPIO_GPIO4_INPUT _MK_GPIO_INPUT(GPIO_TIM5_CH2IN) +//#define GPIO_GPIO5_INPUT _MK_GPIO_INPUT(GPIO_TIM1_CH1IN) + +#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR)) + +#define GPIO_GPIO0_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM3_CH3OUT) +#define GPIO_GPIO1_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM3_CH4OUT) +#define GPIO_GPIO2_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM2_CH3OUT) +#define GPIO_GPIO3_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM2_CH4OUT) +//#define GPIO_GPIO4_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM5_CH2OUT) +//#define GPIO_GPIO5_OUTPUT _MK_GPIO_OUTPUT(GPIO_TIM1_CH1OUT) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN5) + +/* PWM + * + * Alternatively CH3/CH4 could be assigned to UART6_TX/RX + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 4 +#define DIRECT_INPUT_TIMER_CHANNELS 4 + +// Has pwm outputs +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +/* High-resolution timer */ +#define HRT_TIMER 4 // T4C1 +#define HRT_TIMER_CHANNEL 1 // use capture/compare channel 1 + +#define HRT_PPM_CHANNEL 3 // capture/compare channel 3 +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) + +#define RC_SERIAL_PORT "/dev/ttyS0" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* + * One RC_IN + * + * GPIO PPM_IN on PB8 T4CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on PA10 UART1 + * The FMU can drive GPIO PPM_IN as an output + */ +// TODO? +//#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) +//#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +//#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) +//#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_CONSOLE_BUFFER_SIZE (1024*2) + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + + +/**************************************************************************************************** + * Name: stm32_usbinitialize + * + * Description: + * Called to configure USB IO. + * + ****************************************************************************************************/ + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/diatone/mamba-f405-mk2/src/i2c.cpp b/boards/diatone/mamba-f405-mk2/src/i2c.cpp new file mode 100644 index 000000000000..3cf7e2e5955c --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), +}; diff --git a/boards/diatone/mamba-f405-mk2/src/init.c b/boards/diatone/mamba-f405-mk2/src/init.c new file mode 100644 index 000000000000..ad2a64fa8b55 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/init.c @@ -0,0 +1,343 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 init.c + * + * mambaf405mk2-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 +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* + * 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 + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * 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) +{ + /* configure the GPIO pins to outputs and keep them low */ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + 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. + */ + + if (status >= 0) { + up_mdelay(400); + } +} + +/************************************************************************************ + * 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 all PWM to Low outputs */ + + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN11); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN13); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN12); /* RSSI analog in (TX of UART4 instead) */ + + // TODO: power peripherals + ///* configure power supply control/sense pins */ + //stm32_configgpio(GPIO_PERIPH_3V3_EN); + //stm32_configgpio(GPIO_VDD_BRICK_VALID); + //stm32_configgpio(GPIO_VDD_USB_VALID); + + // TODO: 3v3 Sensor? + ///* Start with Sensor voltage off We will enable it + // * in board_app_initialize + // */ + //stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + + // TODO: SBUS inversion? SPEK power? + //stm32_configgpio(GPIO_SBUS_INV); + //stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + // TODO: $$$ Unused? + //stm32_configgpio(GPIO_8266_GPIO0); + //stm32_configgpio(GPIO_8266_PD); + //stm32_configgpio(GPIO_8266_RST); + + /* Safety - led don in led driver */ + + // TODO: unused? + //stm32_configgpio(GPIO_BTN_SAFETY); + + // TODO: RSSI + //stm32_configgpio(GPIO_RSSI_IN); + + stm32_configgpio(GPIO_PPM_IN); + + /* configure SPI all interfaces GPIO */ + + 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. + * + ****************************************************************************/ + +static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; +static struct spi_dev_s *spi3; + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "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_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + + + /* Configure SPI-based devices */ + + // SPI1: MPU6000 + spi1 = stm32_spibus_initialize(1); + + if (!spi1) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); + led_on(LED_BLUE); + return -ENODEV; + } + + /* Default SPI1 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi1, 10000000); + SPI_SETBITS(spi1, 8); + SPI_SETMODE(spi1, SPIDEV_MODE3); + up_udelay(20); + + // SPI2: SDCard + /* Get the SPI port for the microSD slot */ + spi2 = stm32_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi2) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + led_on(LED_BLUE); + return -ENODEV; + } + + /* Now bind the SPI interface to the MMCSD driver */ + int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi2); + + if (result != OK) { + led_on(LED_BLUE); + syslog(LOG_ERR, "[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); + return -ENODEV; + } + + up_udelay(20); + + + // SPI3: OSD / Baro + spi3 = stm32_spibus_initialize(3); + + if (!spi3) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 3\n"); + led_on(LED_BLUE); + return -ENODEV; + } + + /* Copied from fmu-v4 + * Default SPI3 to 12MHz and de-assert the known chip selects. + * MS5611 has max SPI clock speed of 20MHz + */ + + // BMP280 max SPI speed is 10 MHz + SPI_SETFREQUENCY(spi3, 10 * 1000 * 1000); + SPI_SETBITS(spi3, 8); + SPI_SETMODE(spi3, SPIDEV_MODE3); + up_udelay(20); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {1, 16 * 1024, 0x08004000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + 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); + return -ENODEV; + } + +#endif + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/diatone/mamba-f405-mk2/src/led.c b/boards/diatone/mamba-f405-mk2/src/led.c new file mode 100644 index 000000000000..463474a8fdaf --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/led.c @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 led.c + * + * omnibusf4sd LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#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 + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, +}; + +__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]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + if (led == 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + if (led == 0) { + phy_set_led(led, !stm32_gpioread(g_ledmap[led])); + } +} + +__EXPORT void board_autoled_initialize(void) +{ + /* Configure LED1 GPIO for output */ + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void board_autoled_on(int led) +{ + if (led == 1) { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void board_autoled_off(int led) +{ + if (led == 1) { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } +} diff --git a/boards/diatone/mamba-f405-mk2/src/spi.cpp b/boards/diatone/mamba-f405-mk2/src/spi.cpp new file mode 100644 index 000000000000..f744b7fd697f --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/spi.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortB, GPIO::Pin3}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/diatone/mamba-f405-mk2/src/timer_config.cpp b/boards/diatone/mamba-f405-mk2/src/timer_config.cpp new file mode 100644 index 000000000000..8ee6104f5011 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/timer_config.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/diatone/mamba-f405-mk2/src/usb.c b/boards/diatone/mamba-f405-mk2/src/usb.c new file mode 100644 index 000000000000..f02ec76bb800 --- /dev/null +++ b/boards/diatone/mamba-f405-mk2/src/usb.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#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 omnibusf4sd 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_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#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/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake deleted file mode 100644 index 6e401d6c64b3..000000000000 --- a/boards/emlid/navio2/default.cmake +++ /dev/null @@ -1,108 +0,0 @@ -add_definitions( - -D__PX4_LINUX -) - -px4_add_board( - VENDOR emlid - MODEL navio2 - LABEL default - PLATFORM posix - ARCHITECTURE cortex-a53 - ROMFSROOT px4fmu_common - TOOLCHAIN arm-linux-gnueabihf - TESTING - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/mpu9250 - imu/st/lsm9ds1 - linux_pwm_out - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - magnetometer/lsm9ds1_mag - pwm_out_sim - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - #simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - dyn - esc_calib - led_control - mixer - motor_ramp - motor_test - param - perf - pwm - sd_bench - #serial_test - system_time - shutdown - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board new file mode 100644 index 000000000000..0a5409e8c3a3 --- /dev/null +++ b/boards/emlid/navio2/default.px4board @@ -0,0 +1,88 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_LINUX=y +CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf" +CONFIG_BOARD_ARCHITECTURE="cortex-a53" +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IMU_ST_LSM9DS1=y +CONFIG_DRIVERS_LINUX_PWM_OUT=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_FIXEDWING_CONTROL=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y +CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/emlid/navio2/init/rc.board_defaults b/boards/emlid/navio2/init/rc.board_defaults new file mode 100644 index 000000000000..65348c4f846e --- /dev/null +++ b/boards/emlid/navio2/init/rc.board_defaults @@ -0,0 +1,8 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 + diff --git a/boards/emlid/navio2/src/board_config.h b/boards/emlid/navio2/src/board_config.h index 6443bb9c6103..6d8c1eff3789 100644 --- a/boards/emlid/navio2/src/board_config.h +++ b/boards/emlid/navio2/src/board_config.h @@ -42,16 +42,17 @@ #define BOARD_OVERRIDE_UUID "RPIID00000000000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_RPI -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - #define BOARD_MAX_LEDS 1 // Number of external LED's this board has // I2C +#define CONFIG_I2C 1 #define PX4_NUMBER_I2C_BUSES 1 +// SPI +#define CONFIG_SPI 1 + // ADC channels: // A0 - board voltage (shows 5V) // A1 - servo rail voltage diff --git a/boards/freefly/can-rtk-gps/canbootloader.px4board b/boards/freefly/can-rtk-gps/canbootloader.px4board new file mode 100644 index 000000000000..b0fd876605ab --- /dev/null +++ b/boards/freefly/can-rtk-gps/canbootloader.px4board @@ -0,0 +1,6 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y diff --git a/boards/freefly/can-rtk-gps/default.px4board b/boards/freefly/can-rtk-gps/default.px4board new file mode 100644 index 000000000000..85b1558597dd --- /dev/null +++ b/boards/freefly/can-rtk-gps/default.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=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/freefly/can-rtk-gps/firmware.prototype b/boards/freefly/can-rtk-gps/firmware.prototype new file mode 100644 index 000000000000..3f8a8bf393bd --- /dev/null +++ b/boards/freefly/can-rtk-gps/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 85, + "magic": "PX4FWv1", + "description": "Firmware for the FreeFly RTK GPS", + "image": "", + "build_time": 0, + "summary": "FFRTK", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/freefly/can-rtk-gps/init/rc.board_defaults b/boards/freefly/can-rtk-gps/init/rc.board_defaults new file mode 100644 index 000000000000..82830e4a9efe --- /dev/null +++ b/boards/freefly/can-rtk-gps/init/rc.board_defaults @@ -0,0 +1,6 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default CANNODE_GPS_RTCM 1 diff --git a/boards/freefly/can-rtk-gps/init/rc.board_sensors b/boards/freefly/can-rtk-gps/init/rc.board_sensors new file mode 100644 index 000000000000..e905822d32e2 --- /dev/null +++ b/boards/freefly/can-rtk-gps/init/rc.board_sensors @@ -0,0 +1,14 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +rgbled_ncp5623c -I -b 1 -a 0x39 start + +ist8310 start -I -b 1 -a 0x0E + +bmp388 start -I -b b -a 0x77 + +safety_button start + +gps start -d /dev/ttyS0 -g 115200 -p ubx diff --git a/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..4c8d867815c4 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/canbootloader/defconfig @@ -0,0 +1,67 @@ +# +# 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/freefly/can-rtk-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F722RE=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_POLLED=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=0 +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_SDCLONE_DISABLE=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_STM32F7_CAN1=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=4096 diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h new file mode 100644 index 000000000000..0dbed5a97966 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h @@ -0,0 +1,267 @@ +/************************************************************************************ + * 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. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 16 MHz Crystal + * LSE - not installed + */ + +#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 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 2 <= PLLM <= 63 + * 192 <= PLLN <= 432 + * 192 MHz <= PLL_VCO <= 432MHz + * + * SYSCLK = PLL_VCO / PLLP + * Subject to + * + * PLLP = {2, 4, 6, 8} + * SYSCLK <= 216 MHz + * + * USB OTG FS, SDMMC and RNG Clock = PLL_VCO / PLLQ + * Subject to + * The USB OTG FS requires a 48 MHz clock to work correctly. The SDMMC + * and the random number generator need a frequency lower than or equal + * to 48 MHz to work correctly. + * + * 2 <= PLLQ <= 15 + */ + +/* Highest SYSCLK with USB OTG FS clock = 48 MHz + * + * PLL_VCO = (16,000,000 / 8) * 216 = 432 MHz + * SYSCLK = 432 MHz / 2 = 216 MHz + * USB OTG FS, SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(216) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(9) + +#define STM32_VCO_FREQUENCY ((STM32_HSE_FREQUENCY / 8) * 216) +#define STM32_SYSCLK_FREQUENCY (STM32_VCO_FREQUENCY / 2) +#define STM32_OTGFS_FREQUENCY (STM32_VCO_FREQUENCY / 9) + +/* Configure factors for PLLSAI clock */ + +#define CONFIG_STM32F7_PLLSAI 1 +#define STM32_RCC_PLLSAICFGR_PLLSAIN RCC_PLLSAICFGR_PLLSAIN(192) +#define STM32_RCC_PLLSAICFGR_PLLSAIP RCC_PLLSAICFGR_PLLSAIP(8) +#define STM32_RCC_PLLSAICFGR_PLLSAIQ RCC_PLLSAICFGR_PLLSAIQ(4) +#define STM32_RCC_PLLSAICFGR_PLLSAIR RCC_PLLSAICFGR_PLLSAIR(2) + +/* Configure Dedicated Clock Configuration Register */ + +#define STM32_RCC_DCKCFGR1_PLLI2SDIVQ RCC_DCKCFGR1_PLLI2SDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVQ RCC_DCKCFGR1_PLLSAIDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVR RCC_DCKCFGR1_PLLSAIDIVR(0) +#define STM32_RCC_DCKCFGR1_SAI1SRC RCC_DCKCFGR1_SAI1SEL(0) +#define STM32_RCC_DCKCFGR1_SAI2SRC RCC_DCKCFGR1_SAI2SEL(0) +#define STM32_RCC_DCKCFGR1_TIMPRESRC 0 +#define STM32_RCC_DCKCFGR1_DFSDM1SRC 0 +#define STM32_RCC_DCKCFGR1_ADFSDM1SRC 0 + + + +/* Configure factors for PLLI2S clock */ + +#define CONFIG_STM32F7_PLLI2S 1 +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SP RCC_PLLI2SCFGR_PLLI2SP(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) + +/* Configure Dedicated Clock Configuration Register 2 */ + +#define STM32_RCC_DCKCFGR2_USART1SRC RCC_DCKCFGR2_USART1SEL_APB +#define STM32_RCC_DCKCFGR2_USART2SRC RCC_DCKCFGR2_USART2SEL_APB +#define STM32_RCC_DCKCFGR2_UART4SRC RCC_DCKCFGR2_UART4SEL_APB +#define STM32_RCC_DCKCFGR2_UART5SRC RCC_DCKCFGR2_UART5SEL_APB +#define STM32_RCC_DCKCFGR2_USART6SRC RCC_DCKCFGR2_USART6SEL_APB +#define STM32_RCC_DCKCFGR2_UART7SRC RCC_DCKCFGR2_UART7SEL_APB +#define STM32_RCC_DCKCFGR2_UART8SRC RCC_DCKCFGR2_UART8SEL_APB +#define STM32_RCC_DCKCFGR2_I2C1SRC RCC_DCKCFGR2_I2C1SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C2SRC RCC_DCKCFGR2_I2C2SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C3SRC RCC_DCKCFGR2_I2C3SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C4SRC RCC_DCKCFGR2_I2C4SEL_HSI +#define STM32_RCC_DCKCFGR2_LPTIM1SRC RCC_DCKCFGR2_LPTIM1SEL_APB +#define STM32_RCC_DCKCFGR2_CECSRC RCC_DCKCFGR2_CECSEL_HSI +#define STM32_RCC_DCKCFGR2_CK48MSRC RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_SDMMCSRC RCC_DCKCFGR2_SDMMCSEL_48MHZ +#define STM32_RCC_DCKCFGR2_SDMMC2SRC RCC_DCKCFGR2_SDMMC2SEL_48MHZ +#define STM32_RCC_DCKCFGR2_DSISRC RCC_DCKCFGR2_DSISEL_PHY + + +/* Several prescalers allow the configuration of the two AHB buses, the + * high-speed APB (APB2) and the low-speed APB (APB1) domains. The maximum + * frequency of the two AHB buses is 216 MHz while the maximum frequency of + * the high-speed APB domains is 108 MHz. The maximum allowed frequency of + * the low-speed APB domain is 54 MHz. + */ + +/* AHB clock (HCLK) is SYSCLK (216 MHz) */ + +#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/4 (54 MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (108MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + + +/* FLASH wait states + * + * --------- ---------- ----------- + * VDD MAX SYSCLK WAIT STATES + * --------- ---------- ----------- + * 1.7-2.1 V 180 MHz 8 + * 2.1-2.4 V 216 MHz 9 + * 2.4-2.7 V 216 MHz 8 + * 2.7-3.6 V 216 MHz 7 + * --------- ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 7 + +/* LED definitions ******************************************************************/ +/* The board has numerous LEDs but 1, RED LED + + * 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. + */ + + +/* 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 + * ---------------------- -------------------------- ------ */ + +#define LED_STARTED 0 /* NuttX has been started OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF */ +#define LED_INIRQ 4 /* In an interrupt N/C */ +#define LED_SIGNAL 5 /* In a signal handler 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 */ + + +/* UARTs */ + +#define GPIO_USART2_TX GPIO_USART2_TX_1 /* PA2 */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_1 /* PB10 */ +#define GPIO_USART3_RX GPIO_USART3_RX_1 /* PB11 */ + + +/* CAN */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_2 /* PB8 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_2 /* PB9 */ + + +/* I2C */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 /* PA8 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 /* PC9 */ + + +/* SPI */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 /* PC11 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_3 /* PC12 */ +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_1 /* PB3 */ + diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..ca0d10189fe8 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | SPDIFRX_DT | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | SPDIFRX_CS | SPI3_TX_2 | +| Channel 1 | I2C1_RX | I2C3_RX | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_RX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | - | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| Channel 8 | I2C3_TX | I2C4_RX | - | - | I2C2_TX | - | I2C4_TX | - | +| Channel 9 | - | SPI2_RX | - | - | - | - | SPI2_TX | - | +| | | | | | | | | | +| Usage | SPI3_RX_1 | | | | | USART2_RX | USART2_TX | SPI3_TX_2 | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_1 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | SAI2_B_2 | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | SAI2_A | SPI1_TX_2 | SAI2_B | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDMMC1_1 | - | USART1_RX_2 | SDMMC1_2 | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| Channel 8 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | +| Channel 9 | JPEG_IN | JPEG_OUT | SPI4_TX | JPEG_IN | JPEG_OUT | SPI5_RX | - | - | +| Channel 10 | SAI1_B | SAI2_B | SAI2_A | - | - | - | SAI1_A | - | +| Channel 11 | SDMMC2 | - | QUADSPI | - | - | SDMMC2 | - | - | +| | | | | | | | | | +| Usage | SPI4_RX_1 | TIM8_UP | | | SPI4_TX_2 | TIM1_UP | | | + */ + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// SPI3_RX_1 // DMA1, Stream 0, Channel 0 +// AVAILABLE // DMA1, Stream 1 +// AVAILABLE // DMA1, Stream 1 +// AVAILABLE // DMA1, Stream 3 +// AVAILABLE // DMA1, Stream 4 +// DMAMAP_USART2_RX // DMA1, Stream 5, Channel 4 +// DMAMAP_USART2_TX // DMA1, Stream 6, Channel 4 +// SPI3_TX_2 // DMA1, Stream 7, Channel 0 + + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// AVAILABLE // DMA1, Stream 0 +// AVAILABLE // DMA1, Stream 1 +#define DMAMAP_USART1_RX USART1_RX_1 // DMA1, Stream 2, Channel 4 +// AVAILABLE // DMA1, Stream 3 +// AVAILABLE // DMA1, Stream 4 +// AVAILABLE // DMA1, Stream 5 +// AVAILABLE // DMA1, Stream 6 +// DMAMAP_USART1_TX USART1_TX // DMA1, Stream 7, Channel 5 + diff --git a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..7f6030b46506 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig @@ -0,0 +1,174 @@ +# +# 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_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/freefly/can-rtk-gps/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F722RE=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0016 +CONFIG_CDCACM_PRODUCTSTR="PX4 FreeFly RTK GPS" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="Freefly" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=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_STM32F7_CAN1=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_HSECLOCK=y +CONFIG_STM32F7_RTC_MAGIC=0xfacefeee +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_TIM8=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1100 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 000000000000..f63cd44b6c62 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,155 @@ +/**************************************************************************** + * 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 STM32F722ZE has 512 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F32RE, 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 on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option bytes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F32RE will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F32RE also has 256 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 64 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 176 KiB of SRAM1 beginning at address 0x2001:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2003:c000 + * + * 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 = 0x00200000, LENGTH = 512K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + sram1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K + sram2 (rwx) : ORIGIN = 0x2003c000, LENGTH = 16K +} + +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(.); + } > sram1 AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram1 + + /* 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/freefly/can-rtk-gps/nuttx-config/scripts/script.ld b/boards/freefly/can-rtk-gps/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..6e6424daa070 --- /dev/null +++ b/boards/freefly/can-rtk-gps/nuttx-config/scripts/script.ld @@ -0,0 +1,169 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 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. + * + ****************************************************************************/ + +/* The STM32F32RE has 512 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F32RE, 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 on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option bytes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F32RE will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F32RE also has 256 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 64 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 176 KiB of SRAM1 beginning at address 0x2001:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2003:c000 + * + * 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 = 0x00200000, LENGTH = 512K + flash (rx) : ORIGIN = 0x08010000, LENGTH = 512K-64K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + sram1 (rwx) : ORIGIN = 0x20010000, LENGTH = 176K + sram2 (rwx) : ORIGIN = 0x2003c000, LENGTH = 16K +} + +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(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.*) + *(.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(.); + } > sram1 AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram1 + + /* 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/freefly/can-rtk-gps/src/CMakeLists.txt b/boards/freefly/can-rtk-gps/src/CMakeLists.txt new file mode 100644 index 000000000000..246b51200a34 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/CMakeLists.txt @@ -0,0 +1,66 @@ +############################################################################ +# +# 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 "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + i2c.cpp + led.cpp + ) + 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 + spi.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/freefly/can-rtk-gps/src/board_config.h b/boards/freefly/can-rtk-gps/src/board_config.h new file mode 100644 index 000000000000..8a8bbc272a92 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/board_config.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 + +/* BUTTON *************************************************************************** */ + +#define BUTTON_SAFETY /* PC14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN14|GPIO_EXTI) + +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN6) +#define GPIO_BTN_SAFETY (BUTTON_SAFETY & ~GPIO_EXTI) + + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 2 /* use timer 2 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define BOARD_ENABLE_CONSOLE_BUFFER + +__BEGIN_DECLS + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/freefly/can-rtk-gps/src/boot.c b/boards/freefly/can-rtk-gps/src/boot.c new file mode 100644 index 000000000000..4fc6fa5c4ae7 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/boot.c @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * 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 "led.h" +#include "board.h" + +#include +#include +#include + +#include + +/************************************************************************************ + * 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); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + + stm32_configgpio(GPIO_LED_SAFETY); + +#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, 10, 63, 31, 255), + led(2, autobaud_start, 0, 63, 0, 1), + led(3, autobaud_end, 0, 63, 0, 2), + led(4, allocation_start, 0, 0, 31, 2), + led(5, allocation_end, 0, 63, 31, 3), + led(6, fw_update_start, 15, 63, 31, 3), + led(7, fw_update_erase_fail, 15, 63, 15, 3), + led(8, fw_update_invalid_response, 31, 0, 0, 1), + led(9, fw_update_timeout, 31, 0, 0, 2), + led(a, fw_update_invalid_crc, 31, 0, 0, 4), + led(b, jump_to_app, 0, 63, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red << 3, + i2l[indication].green << 2, + i2l[indication].blue << 3, + i2l[indication].hz); +} + +// I2C Driver wants to use the px4 log + +__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) +{ +} + +// I2C Driver wants to use printf etal (putc,wite) so cut it off here + +int printf(FAR const IPTR char *fmt, ...) +{ + return 0; +} diff --git a/boards/freefly/can-rtk-gps/src/boot_config.h b/boards/freefly/can-rtk-gps/src/boot_config.h new file mode 100644 index 000000000000..d5510142e7d6 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/boot_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 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 5000 +#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 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 + +/* I2C LED needs 8 bytes */ + +#define OPT_SIMPLE_MALLOC_HEAP_SIZE 8 + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER GPIO_BTN_SAFETY diff --git a/boards/freefly/can-rtk-gps/src/can.c b/boards/freefly/can-rtk-gps/src/can.c new file mode 100644 index 000000000000..9e9d09c4f9ed --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/can.c @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * 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. + */ + +#ifdef CONFIG_CAN + +#include "board_config.h" + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.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 diff --git a/boards/freefly/can-rtk-gps/src/i2c.cpp b/boards/freefly/can-rtk-gps/src/i2c.cpp new file mode 100644 index 000000000000..3ee9ff0b623b --- /dev/null +++ b/boards/freefly/can-rtk-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), + initI2CBusInternal(3), +}; diff --git a/boards/freefly/can-rtk-gps/src/init.c b/boards/freefly/can-rtk-gps/src/init.c new file mode 100644 index 000000000000..ff36fa2dc7ae --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/init.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include "arm_internal.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 CAN interface + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + + stm32_configgpio(GPIO_LED_SAFETY); + stm32_configgpio(GPIO_BTN_SAFETY); + + // 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[] = { + {1, 32 * 1024, 0x08008000}, + {2, 32 * 1024, 0x08010000}, + {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 + + //px4_platform_configure(); + + return OK; +} diff --git a/boards/freefly/can-rtk-gps/src/led.cpp b/boards/freefly/can-rtk-gps/src/led.cpp new file mode 100644 index 000000000000..704faa5032b2 --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/led.cpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +#include + +#include +#include + +#include +#include "led.h" + +#include + +#include +#include + +__BEGIN_DECLS +#include "cxx_init.h" +void board_autoled_on(int led); +void board_autoled_off(int led); +__END_DECLS + +using namespace time_literals; +#define MODULE_NAME "LED" + +#define ADDR 0x39 /**< I2C adress of NCP5623C */ + +#define NCP5623_LED_CURRENT 0x20 /**< Current register */ +#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ +#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ +#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ + +#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ +#define NCP5623_LED_OFF 0x00 /**< off */ + + +class RGBLED_NCP5623C : public device::I2C +{ +public: + RGBLED_NCP5623C(const int bus, int bus_frequency, const int address); + virtual ~RGBLED_NCP5623C() = default; + + int init() override; + int probe() override; + + int send_led_rgb(uint8_t red, uint8_t green, uint8_t blue); + + +private: + float _brightness{1.0f}; + float _max_brightness{1.0f}; + + int write(uint8_t reg, uint8_t data); +}; + +RGBLED_NCP5623C::RGBLED_NCP5623C(const int bus, int bus_frequency, const int address) : + I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency) +{ +} + +int +RGBLED_NCP5623C::write(uint8_t reg, uint8_t data) +{ + uint8_t msg[1] = { 0x00 }; + msg[0] = ((reg & 0xe0) | (data & 0x1f)); + + int ret = transfer(&msg[0], 1, nullptr, 0); + + return ret; +} + +int +RGBLED_NCP5623C::init() +{ + int ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +RGBLED_NCP5623C::probe() +{ + return write(NCP5623_LED_CURRENT, 0x00); +} + + +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ +int +RGBLED_NCP5623C::send_led_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + + uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80}; + uint8_t brightness = 0x1f * _max_brightness; + + msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f); + msg[2] = NCP5623_LED_PWM0 | (uint8_t(red * _brightness) & 0x1f); + msg[4] = NCP5623_LED_PWM1 | (uint8_t(green * _brightness) & 0x1f); + msg[6] = NCP5623_LED_PWM2 | (uint8_t(blue * _brightness) & 0x1f); + + return transfer(&msg[0], 7, nullptr, 0); +} + +static RGBLED_NCP5623C instance(1, 100000, ADDR); + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +static uint8_t _rgb[] = {0, 0, 0}; + +static int timerInterrupt(int irq, void *context, void *arg) +{ + putreg16(~getreg16(TMR_REG(STM32_GTIM_SR_OFFSET)), TMR_REG(STM32_GTIM_SR_OFFSET)); + + static int d2 = 1; + (d2++ & 1) ? instance.send_led_rgb(0, 0, 0) : instance.send_led_rgb(_rgb[0], _rgb[1], _rgb[2]); + + return 0; +} + + +void rgb_led(int r, int g, int b, int freqs) +{ + long fosc = TMR_FREQUENCY; + long prescale = 1536; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = false; + + if (!once) { + cxx_initialize(); + + if (instance.init() != PX4_OK) { + return; + } + + 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)); + + putreg32(p0p5s + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + + irq_attach(STM32_IRQ_TIM1CC, timerInterrupt, NULL); + up_enable_irq(STM32_IRQ_TIM1CC); + putreg16(GTIM_DIER_CC1IE, TMR_REG(STM32_GTIM_DIER_OFFSET)); + once = true; + } + + long p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + putreg32(p + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); + putreg32(p, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + _rgb[0] = r; + _rgb[1] = g; + _rgb[2] = b; + + 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)); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +__EXPORT void board_autoled_on(int led) +{ + if (led == LED_ASSERTION || led == LED_PANIC) { + stm32_gpiowrite(GPIO_LED_SAFETY, true); + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +__EXPORT void board_autoled_off(int led) +{ + if (led == LED_ASSERTION || led == LED_PANIC) { + stm32_gpiowrite(GPIO_LED_SAFETY, false); + } + +} diff --git a/boards/freefly/can-rtk-gps/src/led.h b/boards/freefly/can-rtk-gps/src/led.h new file mode 100644 index 000000000000..b68e4aa70d0c --- /dev/null +++ b/boards/freefly/can-rtk-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/freefly/can-rtk-gps/src/spi.cpp b/boards/freefly/can-rtk-gps/src/spi.cpp new file mode 100644 index 000000000000..cf00932464d5 --- /dev/null +++ b/boards/freefly/can-rtk-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::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG, SPI::CS{GPIO::PortA, GPIO::Pin15}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/freefly/can-rtk-gps/src/usb.c b/boards/freefly/can-rtk-gps/src/usb.c new file mode 100644 index 000000000000..587516cb8dda --- /dev/null +++ b/boards/freefly/can-rtk-gps/src/usb.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * 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. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * 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 GPIO_OTGFS_VBUS + 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/freefly/can-rtk-gps/uavcan_board_identity b/boards/freefly/can-rtk-gps/uavcan_board_identity new file mode 100644 index 000000000000..997b26e2ae4c --- /dev/null +++ b/boards/freefly/can-rtk-gps/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +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 85) +set(uavcanblid_name "\"org.freefly.can-rtk-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/holybro/can-gps-v1/canbootloader.cmake b/boards/holybro/can-gps-v1/canbootloader.cmake deleted file mode 100644 index 510604a31c94..000000000000 --- a/boards/holybro/can-gps-v1/canbootloader.cmake +++ /dev/null @@ -1,13 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL can-gps-v1 - LABEL canbootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - DRIVERS - bootloaders -) diff --git a/boards/holybro/can-gps-v1/canbootloader.px4board b/boards/holybro/can-gps-v1/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/holybro/can-gps-v1/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/can-gps-v1/debug.cmake b/boards/holybro/can-gps-v1/debug.cmake deleted file mode 100644 index 7e1ced9e311c..000000000000 --- a/boards/holybro/can-gps-v1/debug.cmake +++ /dev/null @@ -1,37 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL can-gps-v1 - LABEL debug - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - CONSTRAINED_FLASH - ROMFSROOT cannode - UAVCAN_INTERFACES 2 - DRIVERS - adc/board_adc - barometer/bmp388 - bootloaders - gps - imu/bosch/bmi088 - lights/rgbled_ncp5623c - magnetometer/bosch/bmm150 - uavcannode - MODULES - #ekf2 - load_mon - sensors - SYSTEMCMDS - i2cdetect - param - perf - reboot - top - #topic_listener - uorb - ver - work_queue -) diff --git a/boards/holybro/can-gps-v1/default.cmake b/boards/holybro/can-gps-v1/default.cmake deleted file mode 100644 index 2ec355f30e1a..000000000000 --- a/boards/holybro/can-gps-v1/default.cmake +++ /dev/null @@ -1,35 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL can-gps-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - CONSTRAINED_FLASH - ROMFSROOT cannode - UAVCAN_INTERFACES 2 - DRIVERS - adc/board_adc - barometer/bmp388 - bootloaders - gps - imu/bosch/bmi088 - lights/rgbled_ncp5623c - magnetometer/bosch/bmm150 - uavcannode - MODULES - #ekf2 - sensors - SYSTEMCMDS - #i2cdetect - param - #perf - #top - #topic_listener - #uorb - #ver - #work_queue -) diff --git a/boards/holybro/can-gps-v1/default.px4board b/boards/holybro/can-gps-v1/default.px4board new file mode 100644 index 000000000000..60ae33341f21 --- /dev/null +++ b/boards/holybro/can-gps-v1/default.px4board @@ -0,0 +1,30 @@ +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_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=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/can-gps-v1/init/rc.board_defaults b/boards/holybro/can-gps-v1/init/rc.board_defaults index 3882c0a2429e..c888bbc68654 100644 --- a/boards/holybro/can-gps-v1/init/rc.board_defaults +++ b/boards/holybro/can-gps-v1/init/rc.board_defaults @@ -3,4 +3,4 @@ # board specific defaults #------------------------------------------------------------------------------ -rgbled_ncp5623c -I -b 1 -a 0x39 start +rgbled_ncp5623c -I -b 2 -a 0x39 start diff --git a/boards/holybro/can-gps-v1/init/rc.board_sensors b/boards/holybro/can-gps-v1/init/rc.board_sensors index fbc9fa43a43a..9adcd321dd82 100644 --- a/boards/holybro/can-gps-v1/init/rc.board_sensors +++ b/boards/holybro/can-gps-v1/init/rc.board_sensors @@ -5,13 +5,12 @@ gps start -d /dev/ttyS0 -g 38400 -p ubx -# TODO:Check for Correct Rotations -# Internal SPI BMI088 -bmi088 -A -R 0 -s start -bmi088 -G -R 0 -s start + +# Internal SPI ICM20649 +icm20649 -s -b 1 -R 8 start # Internal Baro bmp388 -I -b 1 -a 0x77 start # Internal magnetometer on I2c -bmm150 -I -b 1 start +bmm150 -I -b 1 -R 2 start diff --git a/boards/holybro/can-gps-v1/nuttx-config/canbootloader/defconfig b/boards/holybro/can-gps-v1/nuttx-config/canbootloader/defconfig index 40dffb67e84d..96608932d1d4 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/canbootloader/defconfig +++ b/boards/holybro/can-gps-v1/nuttx-config/canbootloader/defconfig @@ -7,7 +7,7 @@ # CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/can-gps-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -36,17 +36,10 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=0 +CONFIG_FS_PROCFS_MAX_TASKS=0 CONFIG_MM_REGIONS=2 CONFIG_NAME_MAX=0 CONFIG_NUNGET_CHARS=0 -CONFIG_NXFONTS_DISABLE_16BPP=y -CONFIG_NXFONTS_DISABLE_1BPP=y -CONFIG_NXFONTS_DISABLE_24BPP=y -CONFIG_NXFONTS_DISABLE_2BPP=y -CONFIG_NXFONTS_DISABLE_32BPP=y -CONFIG_NXFONTS_DISABLE_4BPP=y -CONFIG_NXFONTS_DISABLE_8BPP=y CONFIG_PREALLOC_TIMERS=0 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=262144 diff --git a/boards/holybro/can-gps-v1/nuttx-config/include/board.h b/boards/holybro/can-gps-v1/nuttx-config/include/board.h index d2361d3b5b45..c905886c7576 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/include/board.h +++ b/boards/holybro/can-gps-v1/nuttx-config/include/board.h @@ -128,34 +128,34 @@ /* UARTs */ -#define GPIO_USART1_TX /* PA15 */ GPIO_USART1_TX_3 -#define GPIO_USART1_RX /* PA10 */ GPIO_USART1_RX_1 +#define GPIO_USART1_TX /* PA9 */ GPIO_USART1_TX_1 +#define GPIO_USART1_RX /* PA10 */ GPIO_USART1_RX_1 -#define GPIO_USART2_TX /* PA2 */ GPIO_USART2_TX_1 -#define GPIO_USART2_RX /* PA3 */ GPIO_USART2_RX_1 +#define GPIO_USART2_TX /* PA2 */ GPIO_USART2_TX_1 +#define GPIO_USART2_RX /* PA3 */ GPIO_USART2_RX_1 /* CAN */ -#define GPIO_CAN1_TX /* PB9 */ GPIO_CAN1_TX_2 -#define GPIO_CAN1_RX /* PB8 */ GPIO_CAN1_RX_2 +#define GPIO_CAN1_TX /* PA12 */ GPIO_CAN1_TX_1 +#define GPIO_CAN1_RX /* PA11 */ GPIO_CAN1_RX_1 -#define GPIO_CAN2_TX /* PB13 */ GPIO_CAN2_TX_1 -#define GPIO_CAN2_RX /* PB12 */ GPIO_CAN2_RX_2 +#define GPIO_CAN2_TX /* PB13 */ GPIO_CAN2_TX_1 +#define GPIO_CAN2_RX /* PB12 */ GPIO_CAN2_RX_2 /* I2C */ -#define GPIO_MCU_I2C1_SCL -#define GPIO_MCU_I2C1_SDA +#define GPIO_I2C1_SCL /* PB6 */ GPIO_I2C1_SCL_1 +#define GPIO_I2C1_SDA /* PB7 */ GPIO_I2C1_SDA_1 -#define GPIO_I2C1_SCL /* PB6 */ GPIO_I2C1_SCL_1 -#define GPIO_I2C1_SDA /* PB7 */ GPIO_I2C1_SDA_1 +#define GPIO_I2C2_SCL /* PB10 */ GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA /* PB3 */ GPIO_I2C2_SDA_3 /* SPI */ #define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) #define GPIO_SPI1_MISO /* PA6 */ GPIO_SPI1_MISO_1 -#define GPIO_SPI1_MOSI /* PA7 */ GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_MOSI /* PB5 */ GPIO_SPI1_MOSI_2 #define GPIO_SPI1_SCK /* PA5 */ ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) #endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig index 31f71e8355cf..dede82a525f5 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig @@ -15,12 +15,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_DMACAPABLE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/can-gps-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -30,16 +31,12 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x004f -CONFIG_CDCACM_PRODUCTSTR="PX4 HolyBro CAN GPS" -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="HolyBro" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y @@ -75,18 +72,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -102,7 +94,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -117,8 +108,7 @@ CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y @@ -136,8 +126,8 @@ CONFIG_STM32_DMA2=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_OTGFS=y CONFIG_STM32_PWR=y CONFIG_STM32_RTC=y CONFIG_STM32_RTC_HSECLOCK=y @@ -167,7 +157,6 @@ CONFIG_USART2_RXBUFSIZE=600 CONFIG_USART2_RXDMA=y CONFIG_USART2_SERIAL_CONSOLE=y CONFIG_USART2_TXBUFSIZE=2500 -CONFIG_USBDEV=y CONFIG_USEC_PER_TICK=1000 CONFIG_USERMAIN_STACKSIZE=4096 CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/holybro/can-gps-v1/src/CMakeLists.txt b/boards/holybro/can-gps-v1/src/CMakeLists.txt index 7e6b7a4b4e8c..52006d2bd296 100644 --- a/boards/holybro/can-gps-v1/src/CMakeLists.txt +++ b/boards/holybro/can-gps-v1/src/CMakeLists.txt @@ -68,6 +68,5 @@ else() nuttx_arch nuttx_drivers px4_layer - arch_io_pins ) endif() diff --git a/boards/holybro/can-gps-v1/src/board_config.h b/boards/holybro/can-gps-v1/src/board_config.h index 78339042c7e1..dcf5a9e3b570 100644 --- a/boards/holybro/can-gps-v1/src/board_config.h +++ b/boards/holybro/can-gps-v1/src/board_config.h @@ -50,17 +50,6 @@ // todo:NCP5623 datasheet says 0x38 driver says 0x39 - needs testing /* GPIO *************************************************************************** */ - -/* LEDs are driven with push open drain to support Anode to 3.3V */ - -#define GPIO_MCU_NLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) -#define GPIO_MCU_NLED_BLUE /* PB2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN2) -#define GPIO_MCU_NLED_GREEN /* PB10 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) - -#define BOARD_HAS_CONTROL_STATUS_LEDS 1 -#define BOARD_OVERLOAD_LED LED_RED -#define BOARD_ARMED_STATE_LED LED_BLUE - /* * ADC channels * @@ -80,18 +69,18 @@ /* Define Channel numbers must match above GPIO pin IN(n)*/ -#define ADC_HW_VER_SENSE_CHANNEL /* PA0 */ ADC1_CH(0) -#define ADC_HW_REV_SENSE_CHANNEL /* PA1 */ ADC1_CH(1) +#define ADC_HW_REV_SENSE_CHANNEL /* PA0 */ ADC1_CH(0) +#define ADC_HW_VER_SENSE_CHANNEL /* PA1 */ ADC1_CH(1) #define ADC_CHANNELS \ - ((1 << ADC_HW_VER_SENSE_CHANNEL) | \ - (1 << ADC_HW_REV_SENSE_CHANNEL)) + ((1 << ADC_HW_REV_SENSE_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL)) /* HW Version and Revision drive signals Default to 1 to detect */ #define BOARD_HAS_HW_VERSIONING -#define GPIO_HW_VER_REV_DRIVE /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) +#define GPIO_HW_VER_REV_DRIVE /* PB1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) #define GPIO_HW_REV_SENSE /* PA0 */ ADC1_GPIO(0) #define GPIO_HW_VER_SENSE /* PA1 */ ADC1_GPIO(1) #define HW_INFO_INIT {'C','A','N','G','P','S','x', 'x',0} @@ -104,43 +93,54 @@ * * Silent mode control */ -#define GPIO_MCU_CAN1_SILENT /* PB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MCU_CAN1_SILENT /* PB2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2) #define GPIO_MCU_CAN2_SILENT /* PA4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4) -// Spi pinning is in spi.ccp +// SPI pinning for ICM20649 is in spi.ccp -// GPIO_MCU_SPI1_DRDY /* PA8 */ -// GPIO_MCU_SPI1_NCS_GYRO /* PB14 */ -// GPIO_MCU_SPI1_NCS_ACC /* PB15 */ +// GPIO_MCU_SPI1_DRDY /* PB0 */ +// GPIO_MCU_SPI1_NCS_ACC /* PA8 */ #define GPIO_SENSOR_3V3_EN /* PC15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) -#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_USB_SWITCH_DET /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14) -#define GPIO_NOPT_WAIT_FOR_GETNODEINFO /* PC13 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN13) +#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_SENSOR_3V3_EN, (on_true)) + +#define GPIO_MCU_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6) +#define GPIO_MCU_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7) + +#define GPIO_MCU_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define GPIO_MCU_I2C2_SDA_RESET /* PB3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + + +#define GPIO_NOPT_WAIT_FOR_GETNODEINFO /* PC14 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN14) + +#define GPIO_TIM1_CH1N /* PA7 */ GPIO_TIM1_CH1N_1 /* NLED_RED */ +#define GPIO_TIM1_CH2N /* PB14 */ GPIO_TIM1_CH2N_2 /* NLED_BLUE */ +#define GPIO_TIM1_CH3N /* PB15 */ GPIO_TIM1_CH3N_2 /* NLED_GREEN */ -#define GPIO_GPS_PPS_IN /* PB4 */ GPIO_TIM3_CH1_2 +#define GPIO_TIM2_CH1 /* PA15 */ GPIO_TIM2_CH1_3 /* GPS_PPS_IN */ // todo:needs Driver +#define GPIO_GPS_PPS_IN /* PA15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15) /* High-resolution timer */ -#define HRT_TIMER 3 /* use timer 3 for the HRT */ -#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ +#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 { \ - PX4_ADC_GPIO, \ - GPIO_HW_VER_REV_DRIVE, \ - GPIO_SENSOR_3V3_EN, \ - GPIO_I2C1_SCL_RESET, \ - GPIO_I2C1_SDA_RESET, \ - GPIO_CAN1_TX, \ - GPIO_CAN1_RX, \ - GPIO_CAN2_TX, \ - GPIO_CAN2_RX, \ - GPIO_MCU_CAN1_SILENT, \ - GPIO_MCU_CAN2_SILENT, \ - GPIO_USB_SWITCH_DET, \ - GPIO_NOPT_WAIT_FOR_GETNODEINFO, \ - GPIO_GPS_PPS_IN /* todo:needs driver */ \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_SENSOR_3V3_EN, \ + GPIO_MCU_I2C1_SCL_RESET, \ + GPIO_MCU_I2C1_SDA_RESET, \ + GPIO_MCU_I2C2_SCL_RESET, \ + GPIO_MCU_I2C2_SDA_RESET, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_MCU_CAN1_SILENT, \ + GPIO_MCU_CAN2_SILENT, \ + GPIO_NOPT_WAIT_FOR_GETNODEINFO, \ + GPIO_GPS_PPS_IN \ } __BEGIN_DECLS diff --git a/boards/holybro/can-gps-v1/src/boot.c b/boards/holybro/can-gps-v1/src/boot.c index 4dc967f8f337..cd86a1613bbe 100644 --- a/boards/holybro/can-gps-v1/src/boot.c +++ b/boards/holybro/can-gps-v1/src/boot.c @@ -69,7 +69,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_CAN2_TX); stm32_configgpio(GPIO_MCU_CAN1_SILENT); stm32_configgpio(GPIO_MCU_CAN2_SILENT); - led_init(); // todo:Remove with creation of proper LED driver putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST | RCC_APB1RSTR_CAN2RST, STM32_RCC_APB1RSTR); putreg32(getreg32(STM32_RCC_APB1RSTR) & ~(RCC_APB1RSTR_CAN1RST | RCC_APB1RSTR_CAN2RST), STM32_RCC_APB1RSTR); @@ -168,17 +167,17 @@ typedef begin_packed_struct 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), + led(1, reset, 255, 255, 255, 30), + led(2, autobaud_start, 0, 255, 0, 1), + led(3, autobaud_end, 0, 255, 0, 2), + led(4, allocation_start, 0, 0, 128, 2), + led(5, allocation_end, 0, 255, 128, 3), + led(6, fw_update_start, 64, 255, 128, 3), + led(7, fw_update_erase_fail, 64, 255, 64, 3), + led(8, fw_update_invalid_response, 128, 0, 0, 1), + led(9, fw_update_timeout, 128, 0, 0, 2), + led(a, fw_update_invalid_crc, 128, 0, 0, 4), + led(b, jump_to_app, 0, 255, 0, 10), }; diff --git a/boards/holybro/can-gps-v1/src/i2c.cpp b/boards/holybro/can-gps-v1/src/i2c.cpp index d59c97bcc2f9..8b4552bc7a28 100644 --- a/boards/holybro/can-gps-v1/src/i2c.cpp +++ b/boards/holybro/can-gps-v1/src/i2c.cpp @@ -35,4 +35,5 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusInternal(1), + initI2CBusInternal(2), }; diff --git a/boards/holybro/can-gps-v1/src/init.c b/boards/holybro/can-gps-v1/src/init.c index a22d839cd472..2495283c3608 100644 --- a/boards/holybro/can-gps-v1/src/init.c +++ b/boards/holybro/can-gps-v1/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -65,7 +66,8 @@ #include #include -#include + +#include "led.h" #include @@ -78,20 +80,6 @@ # include #endif -/* - * 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: stm32_boardinitialize * @@ -147,6 +135,8 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { + VDD_3V3_SENSORS_EN(true); + px4_platform_init(); if (OK == board_determine_hw_info()) { @@ -172,36 +162,16 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); - return -ENODEV; } #endif // FLASH_BASED_PARAMS - #if defined(SERIAL_HAVE_RXDMA) - /* set up the serial DMA polling */ + // 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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + 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); - + rgb_led(0, 255, 0, 0); return OK; } diff --git a/boards/holybro/can-gps-v1/src/led.c b/boards/holybro/can-gps-v1/src/led.c index f87ec58e37f0..99c67d9d05b6 100644 --- a/boards/holybro/can-gps-v1/src/led.c +++ b/boards/holybro/can-gps-v1/src/led.c @@ -48,107 +48,17 @@ #include #include -// TODO:This needs a complete rewrite We need to change the HW -// Swap PB0/PB2 and use timer for all LEDS +#include "led.h" #define TMR_BASE STM32_TIM1_BASE #define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN #define TMR_REG(o) (TMR_BASE+(o)) -static uint8_t _off[] = {0, 0, 0}; -static uint8_t _rgb[] = {0, 0, 0}; - -/* - * 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 LED_RED 1 -#define LED_BLUE 0 -#define LED_GREEN 3 - -#define xlat(p) (p) -static uint32_t g_ledmap[] = { - GPIO_MCU_NLED_BLUE, // Indexed by LED_BLUE - GPIO_MCU_NLED_RED, // Indexed by LED_RED, LED_AMBER - 0, // Indexed by LED_SAFETY (defaulted to an input) - GPIO_MCU_NLED_GREEN // Indexed by LED_GREEN -}; - -__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))); -} - - -static void setled(uint8_t *rgb) -{ - phy_set_led(LED_RED, _rgb[0]); - phy_set_led(LED_GREEN, _rgb[1]); - phy_set_led(LED_BLUE, _rgb[2]); -} - - -static int timerInterrupt(int irq, void *context, void *arg) -{ - putreg16(~getreg16(TMR_REG(STM32_GTIM_SR_OFFSET)), TMR_REG(STM32_GTIM_SR_OFFSET)); - - static int d2 = 1; - setled((d2++ & 1) ? _rgb : _off); - return 0; -} - void rgb_led(int r, int g, int b, int freqs) { + long fosc = TMR_FREQUENCY; - long prescale = 1536; + long prescale = 9600; long p1s = fosc / prescale; long p0p5s = p1s / 2; uint16_t val; @@ -156,38 +66,49 @@ void rgb_led(int r, int g, int b, int freqs) if (!once) { once = 1; - setled(_off); /* Enable 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)); - putreg32(p0p5s + 1, TMR_REG(STM32_BTIM_ARR_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_CC3NE | ATIM_CCER_CC3NP | ATIM_CCER_CC2NE | ATIM_CCER_CC2NP | ATIM_CCER_CC1NE | ATIM_CCER_CC1NP, + TMR_REG(STM32_GTIM_CCER_OFFSET)); - irq_attach(STM32_IRQ_TIM1CC, timerInterrupt, NULL); - up_enable_irq(STM32_IRQ_TIM1CC); - putreg16(GTIM_DIER_CC1IE, TMR_REG(STM32_GTIM_DIER_OFFSET)); + stm32_configgpio(GPIO_TIM1_CH1N); + stm32_configgpio(GPIO_TIM1_CH2N); + stm32_configgpio(GPIO_TIM1_CH3N); + + /* master output enable = on */ + + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); } - long p = freqs == 0 ? p1s + 1 : p0p5s / freqs; - putreg32(p + 1, TMR_REG(STM32_BTIM_ARR_OFFSET)); - putreg32(p, TMR_REG(STM32_GTIM_CCR1_OFFSET)); - _rgb[0] = g; - _rgb[1] = r; - _rgb[2] = b; - setled(_rgb); + 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)); @@ -199,4 +120,5 @@ void rgb_led(int r, int g, int b, int freqs) } putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + } diff --git a/boards/holybro/can-gps-v1/src/led.h b/boards/holybro/can-gps-v1/src/led.h index cf12884e567d..b68e4aa70d0c 100644 --- a/boards/holybro/can-gps-v1/src/led.h +++ b/boards/holybro/can-gps-v1/src/led.h @@ -34,5 +34,4 @@ __BEGIN_DECLS void rgb_led(int r, int g, int b, int freqs); -__EXPORT void led_init(void); __END_DECLS diff --git a/boards/holybro/can-gps-v1/src/manifest.c b/boards/holybro/can-gps-v1/src/manifest.c index 08b7a9252743..1beb74589a08 100644 --- a/boards/holybro/can-gps-v1/src/manifest.c +++ b/boards/holybro/can-gps-v1/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -136,7 +138,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/holybro/can-gps-v1/src/spi.cpp b/boards/holybro/can-gps-v1/src/spi.cpp index 754ea6d0b563..b6cc6258d99b 100644 --- a/boards/holybro/can-gps-v1/src/spi.cpp +++ b/boards/holybro/can-gps-v1/src/spi.cpp @@ -37,8 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortB, GPIO::Pin14}, SPI::DRDY{GPIO::PortA, GPIO::Pin8}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin8}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), }, {GPIO::PortC, GPIO::Pin15}), }; diff --git a/boards/holybro/durandal-v1/bootloader.cmake b/boards/holybro/durandal-v1/bootloader.cmake deleted file mode 100644 index a2954fedcbba..000000000000 --- a/boards/holybro/durandal-v1/bootloader.cmake +++ /dev/null @@ -1,20 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL durandal-v1 - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - - - DRIVERS -# bootloader - - MODULES - - SYSTEMCMDS - - EXAMPLES - - ) diff --git a/boards/holybro/durandal-v1/bootloader.px4board b/boards/holybro/durandal-v1/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/holybro/durandal-v1/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake deleted file mode 100644 index 8c12a1e8cfa0..000000000000 --- a/boards/holybro/durandal-v1/default.cmake +++ /dev/null @@ -1,134 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL durandal-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL3:/dev/ttyS4 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi088 - imu/invensense/icm20689 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time -# tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board new file mode 100644 index 000000000000..2feda99a4ccc --- /dev/null +++ b/boards/holybro/durandal-v1/default.px4board @@ -0,0 +1,100 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=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/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin b/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin new file mode 100755 index 000000000000..28ea4d6af7e6 Binary files /dev/null and b/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin differ diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/durandal-v1/init/rc.board_defaults b/boards/holybro/durandal-v1/init/rc.board_defaults index 08a6b82a3d67..49254efb15a4 100644 --- a/boards/holybro/durandal-v1/init/rc.board_defaults +++ b/boards/holybro/durandal-v1/init/rc.board_defaults @@ -1,7 +1,13 @@ #!/bin/sh # -# Holybro Durandal V1 specific board defaults +# board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 18.1 +param set-default BAT2_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 +param set-default BAT2_A_PER_V 36.367515152 + # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 diff --git a/boards/holybro/durandal-v1/init/rc.board_mavlink b/boards/holybro/durandal-v1/init/rc.board_mavlink deleted file mode 100644 index 92ce9ddf39b6..000000000000 --- a/boards/holybro/durandal-v1/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Durandal specific specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors index 99f3d37a748c..9397359b7d34 100644 --- a/boards/holybro/durandal-v1/init/rc.board_sensors +++ b/boards/holybro/durandal-v1/init/rc.board_sensors @@ -11,6 +11,18 @@ icm20689 -R 2 -s start bmi088 -A -R 2 -s start bmi088 -G -R 2 -s start +if ver hwtypecmp VD00 +then + # Internal SPI BMI088 + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start +fi +if ver hwtypecmp VD01 +then + # Internal SPI ICM-20602 + icm20602 -R 2 -s start +fi + # internal compass ist8310 -I -R 10 start diff --git a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig index 4bb3bd9a61e5..fa77567d82b3 100644 --- a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/durandal-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004b CONFIG_CDCACM_PRODUCTSTR="PX4 BL Holybro Durandal Vx" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,9 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 +CONFIG_FS_PROCFS_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +83,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig index 6fea48814481..e849cf0bf255 100644 --- a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/durandal-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004b CONFIG_CDCACM_PRODUCTSTR="PX4 DurandalV1" CONFIG_CDCACM_RXBUFSIZE=600 @@ -71,6 +74,7 @@ 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 @@ -83,7 +87,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -96,18 +99,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,16 +115,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -140,8 +140,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -188,7 +187,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y @@ -207,7 +205,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/holybro/durandal-v1/nuttx-config/scripts/script.ld b/boards/holybro/durandal-v1/nuttx-config/scripts/script.ld index 580f63f0a609..da121fd42279 100644 --- a/boards/holybro/durandal-v1/nuttx-config/scripts/script.ld +++ b/boards/holybro/durandal-v1/nuttx-config/scripts/script.ld @@ -109,16 +109,17 @@ MEMORY { - itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K - flash (rx) : ORIGIN = 0x08020000, LENGTH = 1920K - 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 + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + 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) @@ -156,7 +157,7 @@ SECTIONS *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); - } > flash + } > FLASH /* * Init functions (static constructors and the like) @@ -165,17 +166,17 @@ SECTIONS _sinit = ABSOLUTE(.); KEEP(*(.init_array .init_array.*)) _einit = ABSOLUTE(.); - } > flash + } > FLASH .ARM.extab : { *(.ARM.extab*) - } > flash + } > FLASH __exidx_start = ABSOLUTE(.); .ARM.exidx : { *(.ARM.exidx*) - } > flash + } > FLASH __exidx_end = ABSOLUTE(.); _eronly = ABSOLUTE(.); @@ -186,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > sram AT > flash + + /* 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(.); @@ -195,7 +201,7 @@ SECTIONS *(COMMON) . = ALIGN(4); _ebss = ABSOLUTE(.); - } > sram + } > AXI_SRAM /* Emit the the D3 power domain section for locating BDMA data */ @@ -204,7 +210,7 @@ SECTIONS *(.sram4) . = ALIGN(4); _sram4_heap_start = ABSOLUTE(.); - } > sram4 + } > SRAM4 /* Stabs debugging sections. */ .stab 0 : { *(.stab) } diff --git a/boards/holybro/durandal-v1/src/CMakeLists.txt b/boards/holybro/durandal-v1/src/CMakeLists.txt index 2be96b170df9..8eec4e70c6d4 100644 --- a/boards/holybro/durandal-v1/src/CMakeLists.txt +++ b/boards/holybro/durandal-v1/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers # sdio bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/holybro/durandal-v1/src/board_config.h b/boards/holybro/durandal-v1/src/board_config.h index 815dad59304e..55a3b522388e 100644 --- a/boards/holybro/durandal-v1/src/board_config.h +++ b/boards/holybro/durandal-v1/src/board_config.h @@ -140,12 +140,6 @@ (1 << ADC_HW_REV_SENSE_CHANNEL) | \ (1 << ADC1_3V3_IN_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -162,6 +156,11 @@ #define HW_INFO_INIT_VER 2 #define HW_INFO_INIT_REV 3 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 + +#define VD00 HW_VER_REV(0x0,0x0) // Durandal, Ver 0 Rev 0 +#define VD01 HW_VER_REV(0x0,0x1) // Durandal, Ver 0 Rev 1 + /* CAN Silence * * Silent mode control \ ESC Mux select @@ -174,36 +173,14 @@ * PWM in future */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) - -/* PWM Capture - * - * 6 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PB11 : TIM2_CH4 - * FMU_CAP2 : PB3 : TIM2_CH2 - * FMU_CAP3 : PA5 : TIM2_CH1 - * FMU_CAP4 : PH9 : TIM12_CH2 - * FMU_CAP5 : PH6 : TIM12_CH1 - * FMU_CAP6 : PD14 : TIM4_CH3 - */ - -#define GPIO_TIM2_CH4_IN /* PB11 FMU_CAP3 */ GPIO_TIM2_CH4IN_2 -#define GPIO_TIM2_CH2_IN /* PB3 FMU_CAP2 */ GPIO_TIM2_CH2IN_2 -#define GPIO_TIM2_CH1_IN /* PA5 FMU_CAP1 */ GPIO_TIM2_CH1IN_3 -#define GPIO_TIM12_CH2_IN /* PH9 FMU_CAP4 */ GPIO_TIM12_CH2IN_2 -#define GPIO_TIM12_CH1_IN /* PH6 FMU_CAP5 */ GPIO_TIM12_CH1IN_2 -#define GPIO_TIM4_CH3_IN /* PD14 FMU_CAP6 */ GPIO_TIM4_CH3IN_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 6 +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 5 -#define DIRECT_INPUT_TIMER_CHANNELS 5 +#define DIRECT_PWM_OUTPUT_CHANNELS 10 + +#define BOARD_NUM_IO_TIMERS 4 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4}; /* Power supply control and monitoring GPIOs */ @@ -254,32 +231,6 @@ /* RC Serial port */ -/* Input Capture Channels. */ - -#define INPUT_CAP1_TIMER 2 -#define INPUT_CAP1_CHANNEL /* T2C4 */ 4 -#define GPIO_INPUT_CAP1 /* PB11 */ GPIO_TIM2_CH4_IN - -#define INPUT_CAP2_TIMER 2 -#define INPUT_CAP2_CHANNEL /* T2C2 */ 2 -#define GPIO_INPUT_CAP2 /* PB3 */ GPIO_TIM2_CH2_IN - -#define INPUT_CAP3_TIMER 2 -#define INPUT_CAP3_CHANNEL /* T2C1 */ 1 -#define GPIO_INPU3_CAP1 /* PA5 */ GPIO_TIM2_CH1_IN - -#define INPUT_CAP4_TIMER 12 -#define INPUT_CAP4_CHANNEL /* T12C2*/ 2 -#define GPIO_INPUT_CAP4 /* PH9 */ GPIO_TIM12_CH2_IN - -#define INPUT_CAP5_TIMER 12 -#define INPUT_CAP5_CHANNEL /* T12C1*/ 1 -#define GPIO_INPUT_CAP5 /* PH6 */ GPIO_TIM12_CH1_IN - -#define INPUT_CAP6_TIMER 4 -#define INPUT_CAP6_CHANNEL /* T4C3 */ 3 -#define GPIO_INPUT_CAP6 /* PD14 */ GPIO_TIM4_CH3_IN - /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -331,7 +282,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/holybro/durandal-v1/src/bootloader_main.c b/boards/holybro/durandal-v1/src/bootloader_main.c index 677e5dec2a75..915b7f3d2997 100644 --- a/boards/holybro/durandal-v1/src/bootloader_main.c +++ b/boards/holybro/durandal-v1/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/holybro/durandal-v1/src/init.c b/boards/holybro/durandal-v1/src/init.c index 09cf63df042f..527a8680ae19 100644 --- a/boards/holybro/durandal-v1/src/init.c +++ b/boards/holybro/durandal-v1/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -74,6 +75,8 @@ #include #include +#include + /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ @@ -235,26 +238,12 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } -#if 0 // serial DMA is not yet implemented in NuttX for stm32h7 - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + 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); @@ -275,7 +264,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/holybro/durandal-v1/src/manifest.c b/boards/holybro/durandal-v1/src/manifest.c index 4ca0ef05d1c2..54ab596f5013 100644 --- a/boards/holybro/durandal-v1/src/manifest.c +++ b/boards/holybro/durandal-v1/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -80,7 +82,9 @@ static const px4_hw_mft_item_t hw_mft_list_durandal[] = { }; static px4_hw_mft_list_entry_t mft_lists[] = { - {0x0000, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)}, +// ver_rev + {VD00, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)}, + {VD01, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)}, }; /************************************************************************************ @@ -114,7 +118,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/holybro/durandal-v1/src/spi.cpp b/boards/holybro/durandal-v1/src/spi.cpp index 6d65a49d1307..667fa865c55d 100644 --- a/boards/holybro/durandal-v1/src/spi.cpp +++ b/boards/holybro/durandal-v1/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2020, 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 @@ -35,28 +35,52 @@ #include #include -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), - initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), - }, {GPIO::PortE, GPIO::Pin3}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(HW_VER_REV(0, 0), { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), }), - initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), - }), - initSPIBusExternal(SPI::Bus::SPI5, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + initSPIHWVersion(HW_VER_REV(0, 1), { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), }), }; -static constexpr bool unused = validateSPIConfig(px4_spi_buses); +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/holybro/durandal-v1/src/timer_config.cpp b/boards/holybro/durandal-v1/src/timer_config.cpp index c9bc0acd4665..84915c2b4d9d 100644 --- a/boards/holybro/durandal-v1/src/timer_config.cpp +++ b/boards/holybro/durandal-v1/src/timer_config.cpp @@ -36,6 +36,8 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer1, DMA{DMA::Index1}), initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2), + initIOTimer(Timer::Timer12), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { @@ -44,6 +46,12 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { 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::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin5}), + initIOTimerChannelCapture(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + //initIOTimerChannelCapture(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake deleted file mode 100644 index 612716f27006..000000000000 --- a/boards/holybro/kakutef7/default.cmake +++ /dev/null @@ -1,82 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL kakutef7 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - SERIAL_PORTS - TEL1:/dev/ttyS0 # UART1 - TEL2:/dev/ttyS1 # UART2 -# TEL3:/dev/ttyS2 # UART3 (currently NuttX console) - GPS1:/dev/ttyS3 # UART4 - RC:/dev/ttyS4 # UART6 - # /dev/ttyS5: UART7 (ESC telemetry) - DRIVERS - adc/board_adc - barometer/bmp280 - dshot - gps - imu/invensense/icm20689 - imu/invensense/mpu6000 - #magnetometer - magnetometer/isentek/ist8310 - #optical_flow/px4flow - osd - #pwm_out_sim - pwm_out - rc_input - #telemetry - telemetry/frsky_telemetry - tone_alarm - MODULES - attitude_estimator_q - battery_status - commander - dataman - #ekf2 - events - flight_mode_manager - gyro_calibration - #gyro_fft - land_detector - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - sensors - #temperature_compensation - SYSTEMCMDS - bl_update - dmesg - #dumpfile - #esc_calib - hardfault_log - #i2cdetect - #led_control - mixer - #motor_ramp - #motor_test - #nshterm - param - perf - pwm - reboot - #reflect - #sd_bench - top - #topic_listener - #tune_control - #uorb - #usb_connected - ver - work_queue - ) diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board new file mode 100644 index 000000000000..d82f8bb1da02 --- /dev/null +++ b/boards/holybro/kakutef7/default.px4board @@ -0,0 +1,38 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_EXTERNAL_METADATA=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=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_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PWM=y diff --git a/boards/holybro/kakutef7/bootloader/holybro_kakutef7_bootloader.bin b/boards/holybro/kakutef7/extras/holybro_kakutef7_bootloader.bin similarity index 100% rename from boards/holybro/kakutef7/bootloader/holybro_kakutef7_bootloader.bin rename to boards/holybro/kakutef7/extras/holybro_kakutef7_bootloader.bin diff --git a/boards/holybro/kakutef7/init/rc.board_defaults b/boards/holybro/kakutef7/init/rc.board_defaults index ca65d4cf35ab..2968cb67caca 100644 --- a/boards/holybro/kakutef7/init/rc.board_defaults +++ b/boards/holybro/kakutef7/init/rc.board_defaults @@ -3,6 +3,9 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.9 +param set-default BAT1_A_PER_V 17 + # system_power unavailable param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/holybro/kakutef7/init/rc.board_mavlink b/boards/holybro/kakutef7/init/rc.board_mavlink deleted file mode 100644 index be01959ebc2a..000000000000 --- a/boards/holybro/kakutef7/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# KakuteF7 specific specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index 04934b66a2ad..d97354e91033 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -18,12 +18,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_SPI_CALLBACK is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakutef7/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0016 CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteF7" CONFIG_CDCACM_RXBUFSIZE=600 @@ -74,6 +77,7 @@ CONFIG_FS_PROCFS_EXCLUDE_MOUNT=y CONFIG_FS_PROCFS_EXCLUDE_MOUNTS=y CONFIG_FS_PROCFS_EXCLUDE_PARTITIONS=y CONFIG_FS_PROCFS_EXCLUDE_USAGE=y +CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y CONFIG_FS_ROMFS=y CONFIG_GRAN=y @@ -86,7 +90,7 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 +CONFIG_LIBC_STRERROR_SHORT=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -96,18 +100,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 @@ -119,13 +118,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -141,8 +140,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/holybro/kakutef7/src/board_config.h b/boards/holybro/kakutef7/src/board_config.h index 2822eddb43b8..9254c57c52a6 100644 --- a/boards/holybro/kakutef7/src/board_config.h +++ b/boards/holybro/kakutef7/src/board_config.h @@ -92,15 +92,9 @@ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ (1 << ADC_RSSI_IN_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ -#define BOARD_BATTERY1_V_DIV (10.9f) -#define BOARD_BATTERY1_A_PER_V (17.f) - /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* Tone alarm output */ #define GPIO_TONE_ALARM_IDLE /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) @@ -122,7 +116,6 @@ #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -136,8 +129,6 @@ GPIO_RSSI_IN, \ } -#define BOARD_ENABLE_CONSOLE_BUFFER - #define BOARD_NUM_IO_TIMERS 4 __BEGIN_DECLS diff --git a/boards/holybro/kakutef7/src/init.c b/boards/holybro/kakutef7/src/init.c index 2a41274a3e7e..da2a545c2add 100644 --- a/boards/holybro/kakutef7/src/init.c +++ b/boards/holybro/kakutef7/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -199,23 +200,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -233,7 +222,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi_dev) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); led_on(LED_BLUE); - return -ENODEV; } /* Now bind the SPI interface to the MMCSD driver */ @@ -242,7 +230,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { led_on(LED_BLUE); syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); - return -ENODEV; } up_udelay(20); @@ -261,7 +248,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); led_on(LED_AMBER); - return -ENODEV; } #endif diff --git a/boards/holybro/kakuteh7/bootloader.px4board b/boards/holybro/kakuteh7/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/holybro/kakuteh7/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board new file mode 100644 index 000000000000..f124262a0b84 --- /dev/null +++ b/boards/holybro/kakuteh7/default.px4board @@ -0,0 +1,93 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=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/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin b/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin new file mode 100755 index 000000000000..5a54390368d4 Binary files /dev/null and b/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin differ diff --git a/boards/holybro/kakuteh7/firmware.prototype b/boards/holybro/kakuteh7/firmware.prototype new file mode 100644 index 000000000000..8081ade6354f --- /dev/null +++ b/boards/holybro/kakuteh7/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1048, + "magic": "PX4FWv1", + "description": "Firmware for the KakuteH7 board", + "image": "", + "build_time": 0, + "summary": "KAKUTEH7", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults new file mode 100644 index 000000000000..9adff9289461 --- /dev/null +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -0,0 +1,27 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 11.2 +param set-default BAT1_A_PER_V 59.5 + +# system_power unavailable +param set-default CBRK_SUPPLY_CHK 894281 + +# Select the Generic 250 Racer by default +param set-default SYS_AUTOSTART 4050 + +# use the Q attitude estimator, it works w/o mag or GPS. +param set-default SYS_MC_EST_GROUP 3 +param set-default ATT_ACC_COMP 0 +param set-default ATT_W_ACC 0.4000 +param set-default ATT_W_GYRO_BIAS 0.0000 + +param set-default SYS_HAS_MAG 0 + +# the startup tune is not great on a binary output buzzer, so disable it +param set-default CBRK_BUZZER 782090 + +param set-default IMU_GYRO_RATEMAX 2000 + diff --git a/boards/holybro/kakuteh7/init/rc.board_extras b/boards/holybro/kakuteh7/init/rc.board_extras new file mode 100644 index 000000000000..256633f33c9d --- /dev/null +++ b/boards/holybro/kakuteh7/init/rc.board_extras @@ -0,0 +1,13 @@ +#!/bin/sh +# +# KakuteH7 specific board extras init +#------------------------------------------------------------------------------ + +if ! param compare OSD_ATXXXX_CFG 0 +then + atxxxx start -s +fi + +# DShot telemetry is always on UART7 +dshot telemetry /dev/ttyS5 + diff --git a/boards/holybro/kakuteh7/init/rc.board_sensors b/boards/holybro/kakuteh7/init/rc.board_sensors new file mode 100644 index 000000000000..2711927fe3ed --- /dev/null +++ b/boards/holybro/kakuteh7/init/rc.board_sensors @@ -0,0 +1,13 @@ +#!/bin/sh +# +# Holybro KakuteH7 specific board sensors init +#------------------------------------------------------------------------------ +board_adc start + +# The default IMU is an ICM20689, but there might also be an MPU6000 +if ! mpu6000 -R 6 -s start +then + icm20689 -R 6 -s start +fi + +bmp280 -X start diff --git a/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..5852ec044893 --- /dev/null +++ b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig @@ -0,0 +1,93 @@ +# +# 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/holybro/kakuteh7/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=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0050 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL Holybro KakuteH7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Holybro" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=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_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/holybro/kakuteh7/nuttx-config/include/board.h b/boards/holybro/kakuteh7/nuttx-config/include/board.h new file mode 100644 index 000000000000..83245ff925a3 --- /dev/null +++ b/boards/holybro/kakuteh7/nuttx-config/include/board.h @@ -0,0 +1,419 @@ +/************************************************************************************ + * + * Copyright (C) 2016-2019 Gregory Nutt. 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 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. + * + ************************************************************************************/ +#pragma once + +/************************************************************************************ + * 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 holybro KakuteH7 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 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 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 holybro KakuteH7 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_NLEDS 1 + +#define BOARD_LED_RED BOARD_LED1 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) + +/* 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_2 /* PD6 */ +#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_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + +#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 */ + +/* SPI + * SPI1 SD Card + * SPI2 is OSD AT7456E + * SPI4 is IMU + */ + +#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_4 /* PB13 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE5 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ + +/* 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) + +/* SDMMC1 + * + * VDD 3.3 + * GND + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * GPIO_SDMMC1_NCD PG0 + */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + diff --git a/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..1954ce3e6806 --- /dev/null +++ b/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ + +#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ + diff --git a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..9a402cc427c8 --- /dev/null +++ b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig @@ -0,0 +1,223 @@ +# +# 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_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/kakuteh7/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="PX4 KakuteH7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Holybro" +CONFIG_CLOCK_MONOTONIC=y +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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=1 +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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=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_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_UART4=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_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +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_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=2500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_SERIAL_CONSOLE=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld b/boards/holybro/kakuteh7/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..6f9f9e1b5f24 --- /dev/null +++ b/boards/holybro/kakuteh7/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 KakuteH7 uses an STM32H743VI 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 STM32H743VI, 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 KakuteH7 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 STM32H743VI 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/holybro/kakuteh7/nuttx-config/scripts/script.ld b/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..aa5405fd5f7a --- /dev/null +++ b/boards/holybro/kakuteh7/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * 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 KakuteH7 uses an STM32H743VI 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 STM32H743VI, 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 KakuteH7 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 STM32H743VI 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) + +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/holybro/kakuteh7/src/CMakeLists.txt b/boards/holybro/kakuteh7/src/CMakeLists.txt new file mode 100644 index 000000000000..26fc9de8fc76 --- /dev/null +++ b/boards/holybro/kakuteh7/src/CMakeLists.txt @@ -0,0 +1,66 @@ +############################################################################ +# +# 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_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + 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 + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/holybro/kakuteh7/src/board_config.h b/boards/holybro/kakuteh7/src/board_config.h new file mode 100644 index 000000000000..808ba7d9472b --- /dev/null +++ b/boards/holybro/kakuteh7/src/board_config.h @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * holybro KakuteH7 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Configuration ************************************************************************************/ + +# define BOARD_HAS_USB_VALID 1 +# define BOARD_HAS_NBAT_V 1 +# define BOARD_HAS_NBAT_I 1 + +/* Holybro KakuteH7 GPIOs ************************************************************************/ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +#define GPIO_nLED_RED /* PC2 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED + +/* + * 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) + +/* 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, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* 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_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL)) + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define BOARD_NUM_IO_TIMERS 4 + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_TONE_ALARM_GPIO /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 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 4 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +/* RC Serial port */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ + +#define PWMIN_TIMER 5 +#define PWMIN_TIMER_CHANNEL /* T5C1 */ 1 +#define GPIO_PWM_IN /* PA0 */ GPIO_TIM5_CH1IN + +#define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5) + +#define GPIO_RF_SWITCH /* PE13 */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13) + +/* Power switch controls ******************************************************/ + +#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_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_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)) + +/* Board never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_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 */ + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_RSSI_IN, \ + GPIO_RF_SWITCH, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +// not working on h7 yet, due to ECC +//#define FLASH_BASED_PARAMS + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +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/holybro/kakuteh7/src/bootloader_main.c b/boards/holybro/kakuteh7/src/bootloader_main.c new file mode 100644 index 000000000000..915b7f3d2997 --- /dev/null +++ b/boards/holybro/kakuteh7/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019, 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/holybro/kakuteh7/src/hw_config.h b/boards/holybro/kakuteh7/src/hw_config.h new file mode 100644 index 000000000000..01f1a805fa89 --- /dev/null +++ b/boards/holybro/kakuteh7/src/hw_config.h @@ -0,0 +1,125 @@ +/* + * 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 3000 +#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 BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1048 +#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_RED +#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/holybro/kakuteh7/src/i2c.cpp b/boards/holybro/kakuteh7/src/i2c.cpp new file mode 100644 index 000000000000..e9f26f9a9ee5 --- /dev/null +++ b/boards/holybro/kakuteh7/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; diff --git a/boards/holybro/kakuteh7/src/init.c b/boards/holybro/kakuteh7/src/init.c new file mode 100644 index 000000000000..a9fe66548e01 --- /dev/null +++ b/boards/holybro/kakuteh7/src/init.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 init.c + * + * PX4FMU-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. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#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) +{ +} + +/************************************************************************************ + * 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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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)); + + board_control_spi_sensors_power_configgpio(); + + /* Turn bluetooth off by default (no mavlink support yet) */ + px4_arch_gpiowrite(GPIO_RF_SWITCH, 0); + + /* 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 */ + board_control_spi_sensors_power(true, 0xffff); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + /* configure SPI interfaces */ + + stm32_spiinitialize(); + + /* 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); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + /* Get the SPI port for the microSD slot */ + struct spi_dev_s *spi_dev = stm32_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi_dev) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + led_on(LED_BLUE); + } + + /* Now bind the SPI interface to the MMCSD driver */ + int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi_dev); + + if (result != OK) { + led_on(LED_BLUE); + syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + } + + up_udelay(20); + + +#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 */ + 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 + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/holybro/kakuteh7/src/led.c b/boards/holybro/kakuteh7/src/led.c new file mode 100644 index 000000000000..945c383eb727 --- /dev/null +++ b/boards/holybro/kakuteh7/src/led.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#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_RED, // Indexed by BOARD_LED_RED +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER +}; + +#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/holybro/kakuteh7/src/spi.cpp b/boards/holybro/kakuteh7/src/spi.cpp new file mode 100644 index 000000000000..d188e0470065 --- /dev/null +++ b/boards/holybro/kakuteh7/src/spi.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 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(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortA, GPIO::Pin4}) + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/holybro/kakuteh7/src/timer_config.cpp b/boards/holybro/kakuteh7/src/timer_config.cpp new file mode 100644 index 000000000000..20eb0f15d858 --- /dev/null +++ b/boards/holybro/kakuteh7/src/timer_config.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel3}, {GPIO::PortC, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel4}, {GPIO::PortC, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/holybro/kakuteh7/src/usb.c b/boards/holybro/kakuteh7/src/usb.c new file mode 100644 index 000000000000..9d5915c0e6d1 --- /dev/null +++ b/boards/holybro/kakuteh7/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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#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 PX4FMU 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/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake deleted file mode 100644 index 233f7d620e88..000000000000 --- a/boards/holybro/pix32v5/default.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR holybro - MODEL pix32v5 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL3:/dev/ttyS4 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board new file mode 100644 index 000000000000..4d09a3a008e5 --- /dev/null +++ b/boards/holybro/pix32v5/default.px4board @@ -0,0 +1,113 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_IO="px4_io-v2_default" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/holybro/pix32v5/bootloader/holybro_pix32v5_bootloader.bin b/boards/holybro/pix32v5/extras/holybro_pix32v5_bootloader.bin similarity index 100% rename from boards/holybro/pix32v5/bootloader/holybro_pix32v5_bootloader.bin rename to boards/holybro/pix32v5/extras/holybro_pix32v5_bootloader.bin diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/pix32v5/init/rc.board_defaults b/boards/holybro/pix32v5/init/rc.board_defaults index 3b99b63eb3e3..c2300a50a770 100644 --- a/boards/holybro/pix32v5/init/rc.board_defaults +++ b/boards/holybro/pix32v5/init/rc.board_defaults @@ -3,5 +3,11 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 18.1 +param set-default BAT2_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 +param set-default BAT2_A_PER_V 36.367515152 + rgbled_pwm start safety_button start diff --git a/boards/holybro/pix32v5/init/rc.board_mavlink b/boards/holybro/pix32v5/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/holybro/pix32v5/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig index d934bc3d2afa..898ff192dda8 100644 --- a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig +++ b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/holybro/pix32v5/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -38,6 +39,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -45,6 +47,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004E CONFIG_CDCACM_PRODUCTSTR="PX4 PIX32V5" CONFIG_CDCACM_RXBUFSIZE=600 @@ -71,6 +74,7 @@ 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 @@ -83,7 +87,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +98,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -116,16 +114,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -140,8 +140,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/holybro/pix32v5/src/board_config.h b/boards/holybro/pix32v5/src/board_config.h index 98e5e6f9b198..61adf5d679f8 100644 --- a/boards/holybro/pix32v5/src/board_config.h +++ b/boards/holybro/pix32v5/src/board_config.h @@ -173,12 +173,6 @@ (1 << ADC1_SPARE_1_CHANNEL)) #endif -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -209,22 +203,7 @@ * PWM in future */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) - -/* PWM Capture - * - * 3 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PA5 : TIM2_CH1 - * FMU_CAP2 : PB3 : TIM2_CH2 - * FMU_CAP3 : PB11 : TIM2_CH4 - */ -#define GPIO_TIM2_CH1_IN /* PA5 T22C1 FMU_CAP1 */ GPIO_TIM2_CH1IN_3 -#define GPIO_TIM2_CH2_IN /* PB3 T22C2 FMU_CAP2 */ GPIO_TIM2_CH2IN_2 -#define GPIO_TIM2_CH4_IN /* PB11 T22C4 FMU_CAP3 */ GPIO_TIM2_CH4IN_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 3 +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PI0 is nARMED * The GPIO will be set as input while not armed HW will have external HW Pull UP. @@ -237,8 +216,7 @@ /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 11 #define BOARD_HAS_LED_PWM 1 #define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 @@ -307,19 +285,6 @@ #define RC_SERIAL_PORT "/dev/ttyS4" #define RC_SERIAL_SINGLEWIRE -/* Input Capture Channels. */ -#define INPUT_CAP1_TIMER 2 -#define INPUT_CAP1_CHANNEL /* T4C1 */ 1 -#define GPIO_INPUT_CAP1 /* PA5 */ GPIO_TIM2_CH1_IN - -#define INPUT_CAP2_TIMER 2 -#define INPUT_CAP2_CHANNEL /* T4C2 */ 2 -#define GPIO_INPUT_CAP2 /* PB3 */ GPIO_TIM2_CH2_IN - -#define INPUT_CAP3_TIMER 2 -#define INPUT_CAP3_CHANNEL /* T4C4 */ 4 -#define GPIO_INPUT_CAP3 /* PB11 */ GPIO_TIM2_CH4_IN - /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL /* T4C2 */ 2 @@ -421,7 +386,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -466,7 +430,6 @@ #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/holybro/pix32v5/src/init.c b/boards/holybro/pix32v5/src/init.c index 8b1208020b05..2153838a6075 100644 --- a/boards/holybro/pix32v5/src/init.c +++ b/boards/holybro/pix32v5/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -251,22 +252,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #if defined(SERIAL_HAVE_RXDMA) - /* set up the serial DMA polling */ + // 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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* initial LED state */ @@ -284,7 +272,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/holybro/pix32v5/src/manifest.c b/boards/holybro/pix32v5/src/manifest.c index 32d61234dc8d..2c5375a3c79f 100644 --- a/boards/holybro/pix32v5/src/manifest.c +++ b/boards/holybro/pix32v5/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -162,7 +164,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/holybro/pix32v5/src/timer_config.cpp b/boards/holybro/pix32v5/src/timer_config.cpp index c6e21851f3b1..8bebdca77312 100644 --- a/boards/holybro/pix32v5/src/timer_config.cpp +++ b/boards/holybro/pix32v5/src/timer_config.cpp @@ -50,6 +50,9 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin5}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/matek/gnss-m9n-f4/canbootloader.px4board b/boards/matek/gnss-m9n-f4/canbootloader.px4board new file mode 100644 index 000000000000..fcbf18c4cf7d --- /dev/null +++ b/boards/matek/gnss-m9n-f4/canbootloader.px4board @@ -0,0 +1,7 @@ +CONFIG_PLATFORM_NUTTX=y +CONFIG_BOARD_PLATFORM="nuttx" +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/matek/gnss-m9n-f4/default.px4board b/boards/matek/gnss-m9n-f4/default.px4board new file mode 100644 index 000000000000..230f1e85a54a --- /dev/null +++ b/boards/matek/gnss-m9n-f4/default.px4board @@ -0,0 +1,34 @@ +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_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_MAGNETOMETER_RM3100=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y +CONFIG_SERIAL_PASSTHRU_UBLOX=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/matek/gnss-m9n-f4/firmware.prototype b/boards/matek/gnss-m9n-f4/firmware.prototype new file mode 100755 index 000000000000..41a09063f960 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1014, + "magic": "PX4FWv1", + "description": "Firmware for the MatekM9nf4can cannode board", + "image": "", + "build_time": 0, + "summary": "MateksysGnss-m9n-f4", + "version": "0.1", + "image_size": 0, + "image_maxsize": 983040, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/matek/gnss-m9n-f4/init/rc.board_sensors b/boards/matek/gnss-m9n-f4/init/rc.board_sensors new file mode 100755 index 000000000000..065107a29396 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/init/rc.board_sensors @@ -0,0 +1,12 @@ +#!/bin/sh +# +# Matek M9NF4 CAN specific board sensors init +#------------------------------------------------------------------------------ + +icm20602 -s -R 10 start + +rm3100 -b 2 -s -R 12 start + +dps310 -a 118 -X start + +sensors start diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig new file mode 100644 index 000000000000..a15e2285152b --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig @@ -0,0 +1,64 @@ +# +# 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/matek/gnss-m9n-f4/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_STM32F405RG=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_C99_BOOL8=y +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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=131072 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SDCLONE_DISABLE=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_JTAG_SW_ENABLE=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/include/board.h b/boards/matek/gnss-m9n-f4/nuttx-config/include/board.h new file mode 100755 index 000000000000..27c84f00130f --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/include/board.h @@ -0,0 +1,302 @@ +/************************************************************************************ + * include/arch/board/board.h + * + * Copyright (C) 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Author: Nathan Tsoi + * + * 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 __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include "board_dma_map.h" + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The Mateksys GNSS M9N-F4 board features a single 8MHz crystal. + * + * This is the canonical configuration: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 8000000 (STM32_BOARD_XTAL) + * PLLM : 8 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PLLQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 8MHz + * LSE - 32.768 kHz + */ + +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000ul +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (8,000,000 / 8) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#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/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8-11 are on APB2, others on APB1 + */ + +#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN +#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN +#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN +#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN +#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN +#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN +#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN +#define BOARD_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN +#define BOARD_TIM9_FREQUENCY STM32_APB2_TIM9_CLKIN +#define BOARD_TIM10_FREQUENCY STM32_APB2_TIM10_CLKIN +#define BOARD_TIM11_FREQUENCY STM32_APB2_TIM11_CLKIN +#define BOARD_TIM12_FREQUENCY STM32_APB1_TIM12_CLKIN +#define BOARD_TIM13_FREQUENCY STM32_APB1_TIM13_CLKIN +#define BOARD_TIM14_FREQUENCY STM32_APB1_TIM14_CLKIN + +/* LED definitions ******************************************************************/ +/* 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 stm32_setled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 + +/* LED bits for use with stm32_setleds() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board + * The following definitions describe how NuttX controls the LEDs: + */ + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 */ +#define LED_STACKCREATED 3 /* LED1 + LED2 */ +#define LED_INIRQ 4 /* LED1 */ +#define LED_SIGNAL 5 /* LED2 */ +#define LED_ASSERTION 6 /* LED1 + LED2 */ +#define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* UART1: + * + * PA10 (RX) and PA9 (TX) are broken out on J5 + */ + +#define GPIO_USART1_RX GPIO_USART1_RX_1 +#define GPIO_USART1_TX GPIO_USART1_TX_1 + +/* UART2: + * + * PA10 (RX) and PA9 (TX) are broken out on J5 + */ + +//#define GPIO_USART2_RX GPIO_USART2_RX_1 +//#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* USART3: + * + * PC10 (TX) and PC11 (RX) are broken out on J4 + * + * + * The Silkscreen pin labeled SCL is TX + * MISO is RX + */ +#define GPIO_USART3_RX GPIO_USART3_RX_2 +#define GPIO_USART3_TX GPIO_USART3_TX_2 + +/* UART4: + * + * PA0 (TX) -- Labeled RSSI on the silkscreen is only broken out on a test pad + * on the pro version. It's on a 2.54mm header on other versions + * PA1 (RX) -- Motor 5 out + */ +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +/* UART5: + * + * PC6 (TX) and PC7 (RX) are broken out on J10 + */ + +//#define GPIO_UART5_RX GPIO_UART5_RX_1 +//#define GPIO_UART5_TX GPIO_UART5_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_2 +#define GPIO_CAN1_TX GPIO_CAN1_TX_2 + +/* SPI1: + * CS: PA4 -- configured in board_config.h + * CLK: PA5 + * MISO: PA6 + * MOSI: PA7 + */ + +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 + +/* SPI2: + * SD Card + * CS: PB12 -- configured in board_config.h + * CLK: PB13 + * MISO: PB14 + * MOSI: PB15 + */ + +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 + +/* SPI3: SD CARD + * CS: PC14 -- configured in board_config.h + * CLK: PB3 + * MISO: PB4 + * MOSI: PB5 + */ + +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_1 +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 + +/* + * I2C (external) + * + * SCL: PB10 + * SDA: PB11 + * + * TODO: + * 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_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 + +#endif /* __CONFIG_M9NF4_INCLUDE_BOARD_H */ diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/include/board_dma_map.h b/boards/matek/gnss-m9n-f4/nuttx-config/include/board_dma_map.h new file mode 100755 index 000000000000..81de4f9b2ac5 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/include/board_dma_map.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/* +| DMA1 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | - | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | - | SPI3_TX_2 | +| Channel 1 | I2C1_RX | - | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_RX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | I2C2_EXT_RX | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| | | | | | | | | | +| Usage | SPI3_RX | TIM2_UP_1 | UART4_RX | SPI2_RX | SPI2_TX | | | SPI3_TX | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_1 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | - | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | - | SPI1_TX_2 | - | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDIO | - | USART1_RX_2 | SDIO | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| | | | | | | | | | +| Usage | SPI1_RX_1 | USART6_RX_1 | USART1_RX_1 | SPI1_TX_1 | | | | | + */ + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4 (CONSOLE) +//#define DMAMAP_UART4_RX DMAMAP_UART4_RX // DMA1, Stream 2, Channel 4 (GPS) + +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 (SPI2 RX) +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 (SPI2 TX) +#define DMACHAN_SPI3_RX DMAMAP_SPI3_RX_1 // DMA1, Stream 0, Channel 0 (SPI3 RX SD) +#define DMACHAN_SPI3_TX DMAMAP_SPI3_TX_2 // DMA1, Stream 7, Channel 0 (SPI3 TX SD) + +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3 (SPI1 RX) +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 (SPI1 TX) diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..b2d50ba18849 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig @@ -0,0 +1,194 @@ +# +# 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_OS_API is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CMP is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_SPI_CALLBACK is not set +# CONFIG_STM32_CCMEXCLUDE is not set +# CONFIG_STM32_DMACAPABLE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/gnss-m9n-f4/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_STM32F405RG=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_PRODUCTID=0x1004 +CONFIG_CDCACM_PRODUCTSTR="Matekgnssm9nf4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x120A +CONFIG_CDCACM_VENDORSTR="PX4" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=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_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=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_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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MM_REGIONS=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_MMCSDSPIPORTNO=3 +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_SETSPEED=y +CONFIG_RAM_SIZE=131072 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_PASSTHRU_UBLOX=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_I2C2=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_OTGHS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +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_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI2_DMA_BUFFER=1024 +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI3_DMA=y +CONFIG_STM32_SPI3_DMA_BUFFER=512 +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=1024 +CONFIG_STM32_SYSCFG_IOCOMPENSATION=y +CONFIG_STM32_TIM5=y +CONFIG_STM32_UART4=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=38400 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=300 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/scripts/canbootloader_script.ld b/boards/matek/gnss-m9n-f4/nuttx-config/scripts/canbootloader_script.ld new file mode 100755 index 000000000000..e0510a276da2 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,137 @@ +/**************************************************************************** + * configs/gnss-m9n-f4/scripts/ld.canbootloader + * + * 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 STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb 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 CCM 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 32 KiB of flash is reserved for the bootloader. + * Paramater storage will use the next 32KiB Sector. + */ + +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/matek/gnss-m9n-f4/nuttx-config/scripts/script.ld b/boards/matek/gnss-m9n-f4/nuttx-config/scripts/script.ld new file mode 100755 index 000000000000..70696dfb2cec --- /dev/null +++ b/boards/matek/gnss-m9n-f4/nuttx-config/scripts/script.ld @@ -0,0 +1,147 @@ +/**************************************************************************** + * configs/gnss-m9n-f4/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 STM32F405RG has 1024Kb of FLASH beginning at address 0x0800:0000 and + * 192Kb 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 CCM 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 32 KiB of flash is reserved for the bootloader. + * Paramater storage will use the next 32KiB Sector. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 960K + 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/matek/gnss-m9n-f4/src/CMakeLists.txt b/boards/matek/gnss-m9n-f4/src/CMakeLists.txt new file mode 100755 index 000000000000..d3ba91653aee --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/CMakeLists.txt @@ -0,0 +1,70 @@ +############################################################################ +# +# Copyright (c) 2018-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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + usb.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 + i2c.cpp + led.c + init.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + nuttx_arch + nuttx_drivers + px4_layer + arch_io_pins + drivers__led + ) +endif() diff --git a/boards/matek/gnss-m9n-f4/src/board_config.h b/boards/matek/gnss-m9n-f4/src/board_config.h new file mode 100755 index 000000000000..1b2ddb964f1e --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/board_config.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 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 + * + * M9nf4 can internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* Matek M9nf4can GPIOs ***********************************************************************************/ +/* LEDs */ +// LED1 - PA14 - blue +// LED2 - PA13 - green + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN14) +#define GPIO_LED_BLUE GPIO_LED1 + +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) +#define GPIO_LED_GREEN GPIO_LED2 + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PB7 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN7|GPIO_EXTI) + +#define FLASH_BASED_PARAMS + +#define GPIO_CAN1_SILENT_S0 /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTC|GPIO_PIN5) + +#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 /* PB11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 as HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define BOARD_ENABLE_CONSOLE_BUFFER + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + + +#define PX4_GPIO_INIT_LIST { \ + GPIO_I2C2_SCL_RESET, \ + GPIO_I2C2_SDA_RESET, \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_OTGFS_VBUS \ + } + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + + +/**************************************************************************************************** + * Name: stm32_usbinitialize + * + * Description: + * Called to configure USB IO. + * + ****************************************************************************************************/ + +extern void stm32_usbinitialize(void); + +//extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/matek/gnss-m9n-f4/src/boot.c b/boards/matek/gnss-m9n-f4/src/boot.c new file mode 100755 index 000000000000..3cb180de23fa --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/boot.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 "led.h" +#include + + +/************************************************************************************ + * 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 + * + ****************************************************************************/ +#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/matek/gnss-m9n-f4/src/boot_config.h b/boards/matek/gnss-m9n-f4/src/boot_config.h new file mode 100755 index 000000000000..0a01b668db7b --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/boot_config.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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 5000 +#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 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_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/matek/gnss-m9n-f4/src/can.c b/boards/matek/gnss-m9n-f4/src/can.c new file mode 100755 index 000000000000..acc20252fab0 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/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_arch.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/matek/gnss-m9n-f4/src/i2c.cpp b/boards/matek/gnss-m9n-f4/src/i2c.cpp new file mode 100755 index 000000000000..7aa36d107b0f --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (C) 2020-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(2), +}; diff --git a/boards/matek/gnss-m9n-f4/src/init.c b/boards/matek/gnss-m9n-f4/src/init.c new file mode 100755 index 000000000000..7556da22dc67 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/init.c @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * 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 + * + * Matek M9NF4 CAN -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 +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include "led.h" + +#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 LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI all interfaces 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. + * + ****************************************************************************/ + +static struct spi_dev_s *spi3; + +__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 + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "DMA alloc FAILED\n"); + } + + + stm32_spiinitialize(); + + + spi3 = stm32_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi3) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 3\n"); + rgb_led(255, 0, 0, 0); + return -ENODEV; + } + + int ret = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3); + + if (ret < 0) { + syslog(LOG_ERR, "[boot] Failed to bind SPI port 0 to SD slot 0\n"); + rgb_led(255, 0, 0, 0); + return ret; + } + +#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); + rgb_led(255, 0, 0, 0); + return -ENODEV; + } + +#endif // FLASH_BASED_PARAMS + + rgb_led(0, 255, 0, 0); + + return OK; +} diff --git a/boards/matek/gnss-m9n-f4/src/led.c b/boards/matek/gnss-m9n-f4/src/led.c new file mode 100755 index 000000000000..7cfcbdf35484 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/led.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 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)) + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, +}; + +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_CH1N_1); + stm32_configgpio(GPIO_TIM1_CH2N_1); + stm32_configgpio(GPIO_TIM1_CH3N_1); + + /* 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)); + +} + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + if (led == 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} +/* +__EXPORT void board_autoled_on(int led) { + if (led == 1) { + stm32_gpiowrite(GPIO_LED1, false); + } +} + +__EXPORT void board_autoled_off(int led) { + if (led == 1) { + stm32_gpiowrite(GPIO_LED1, true); + } +}*/ + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + if (led == 0) { + phy_set_led(led, !stm32_gpioread(g_ledmap[led])); + } +} diff --git a/boards/matek/gnss-m9n-f4/src/led.h b/boards/matek/gnss-m9n-f4/src/led.h new file mode 100755 index 000000000000..2968e55c2045 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/led.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (C) 2015-2021 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); +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 diff --git a/boards/matek/gnss-m9n-f4/src/spi.cpp b/boards/matek/gnss-m9n-f4/src/spi.cpp new file mode 100755 index 000000000000..f241900fed62 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/spi.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2018-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_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}, SPI::DRDY{GPIO::PortC, GPIO::Pin4}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_MAG_DEVTYPE_RM3100, SPI::CS{GPIO::PortB, GPIO::Pin12}) + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortC, GPIO::Pin14}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/matek/gnss-m9n-f4/src/timer_config.cpp b/boards/matek/gnss-m9n-f4/src/timer_config.cpp new file mode 100755 index 000000000000..c01af147e247 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/timer_config.cpp @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2018-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::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), +}; + +//constexpr io_timers_channel_mapping_t io_timers_channel_mapping = +// initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/matek/gnss-m9n-f4/src/usb.c b/boards/matek/gnss-m9n-f4/src/usb.c new file mode 100755 index 000000000000..d90fa31c303c --- /dev/null +++ b/boards/matek/gnss-m9n-f4/src/usb.c @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2018-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. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#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 omnibusf4sd 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_STM32_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); + /* XXX We only support device mode + stm32_configgpio(GPIO_OTGFS_PWRON); + stm32_configgpio(GPIO_OTGFS_OVER); + */ +#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/matek/gnss-m9n-f4/uavcan_board_identity b/boards/matek/gnss-m9n-f4/uavcan_board_identity new file mode 100755 index 000000000000..70123f7d91a9 --- /dev/null +++ b/boards/matek/gnss-m9n-f4/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 3) +set(uavcanblid_hw_version_minor 246) +set(uavcanblid_name "\"org.matek.gnss-m9n-f4\"") + +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/matek/h743-mini/bootloader.px4board b/boards/matek/h743-mini/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/matek/h743-mini/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board new file mode 100644 index 000000000000..d7669432a98d --- /dev/null +++ b/boards/matek/h743-mini/default.px4board @@ -0,0 +1,90 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +CONFIG_COMMON_DISTANCE_SENSOR=n +CONFIG_COMMON_LIGHT=n +CONFIG_COMMON_MAGNETOMETER=n +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=n +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM=n +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=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 +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=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=n +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_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_GIMBAL=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=n +CONFIG_SYSTEMCMDS_MFT=n +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_MTD=n +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=n +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=n diff --git a/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin b/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin new file mode 100755 index 000000000000..21c84e1d2b61 Binary files /dev/null and b/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin differ diff --git a/boards/matek/h743-mini/extras/matek_h743-slim_bootloader.bin b/boards/matek/h743-mini/extras/matek_h743-slim_bootloader.bin new file mode 100644 index 000000000000..9a03d6bfa45a Binary files /dev/null and b/boards/matek/h743-mini/extras/matek_h743-slim_bootloader.bin differ diff --git a/boards/matek/h743-mini/firmware.prototype b/boards/matek/h743-mini/firmware.prototype new file mode 100644 index 000000000000..d715678889a3 --- /dev/null +++ b/boards/matek/h743-mini/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1013, + "magic": "PX4FWv1", + "description": "Firmware for the MatekH743-slim board", + "image": "", + "build_time": 0, + "summary": "MatekH743-mini", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/matek/h743-mini/init/rc.board_extras b/boards/matek/h743-mini/init/rc.board_extras new file mode 100644 index 000000000000..a39b81bdc223 --- /dev/null +++ b/boards/matek/h743-mini/init/rc.board_extras @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# if ! param compare OSD_ATXXXX_CFG 0 +# then +# atxxxx start -s +# fi + +atxxxx start -s + + +# DShot telemetry is always on UART7 +# dshot telemetry /dev/ttyS5 + diff --git a/boards/matek/h743-mini/init/rc.board_sensors b/boards/matek/h743-mini/init/rc.board_sensors new file mode 100644 index 000000000000..c37c3b3b5868 --- /dev/null +++ b/boards/matek/h743-mini/init/rc.board_sensors @@ -0,0 +1,22 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal SPI bus ICM-42605 +if ! icm42605 -R 14 -s start +then + # internal SPI bus ICM-20602 + icm20602 -R 12 -s start +fi + +# Internal SPI bus MPU-6000 +mpu6000 -R 12 -s start + +# Internal baro +dps310 -I start -a 118 + +# External mag +qmc5883l -X start -a 13 diff --git a/boards/matek/h743-mini/nuttx-config/Kconfig b/boards/matek/h743-mini/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/matek/h743-mini/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/matek/h743-mini/nuttx-config/bootloader/defconfig b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..b058e087e791 --- /dev/null +++ b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig @@ -0,0 +1,92 @@ +# +# 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/matek/h743-mini/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=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="Matek H743-mini" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART1=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_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/matek/h743-mini/nuttx-config/include/board.h b/boards/matek/h743-mini/nuttx-config/include/board.h new file mode 100644 index 000000000000..4bf2b6e45e0f --- /dev/null +++ b/boards/matek/h743-mini/nuttx-config/include/board.h @@ -0,0 +1,493 @@ +/************************************************************************************ + * 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_MATEKH743SLIM_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MATEKH743SLIM_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 MatekH743-Slim 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_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* 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_2 /* PD6 */ +#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_3 /* PB8 */ +#define GPIO_UART4_TX GPIO_UART4_TX_3 /* PB9 */ + +#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_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* 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 MPU6000 + * SPI2 is MAX7456 + * SPI3 is extern with PD4 and PE2 as CS + * SPI4 is ICM20602 + */ + +#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_3 /* PD7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(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 ADJ_SLEW_RATE(GPIO_SPI2_SCK_4) /* PB13 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_4 /* PB5 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PB3 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_1 /* PE14 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_1) /* PE12 */ + +/* 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_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) + +/* SDMMC1 + * + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + */ + +// #define GPIO_SDMMC1_D0 GPIO_SDMMC1_D0 /* PC8 */ +// #define GPIO_SDMMC1_D1 GPIO_SDMMC1_D1 /* PC9 */ +// #define GPIO_SDMMC1_D2 GPIO_SDMMC1_D2 /* PC10 */ +// #define GPIO_SDMMC1_D3 GPIO_SDMMC1_D3 /* PC11 */ +// #define GPIO_SDMMC1_CK GPIO_SDMMC1_CK /* PC12 */ +// #define GPIO_SDMMC1_CMD GPIO_SDMMC1_CMD /* PD2 */ + + +/* 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_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +# 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_MATEKH743SLIM_INCLUDE_BOARD_H */ diff --git a/boards/matek/h743-mini/nuttx-config/include/board_dma_map.h b/boards/matek/h743-mini/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..693132c389aa --- /dev/null +++ b/boards/matek/h743-mini/nuttx-config/include/board_dma_map.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + + +// DMAMUX2 +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2:83 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2:84 */ diff --git a/boards/matek/h743-mini/nuttx-config/nsh/defconfig b/boards/matek/h743-mini/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..2a9c353c9d68 --- /dev/null +++ b/boards/matek/h743-mini/nuttx-config/nsh/defconfig @@ -0,0 +1,224 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743-mini/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=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="MatekH743-mini" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_FLOWCONTROL_BROKEN=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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +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_SERIAL_CONSOLE=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/matek/h743-mini/nuttx-config/scripts/bootloader_script.ld b/boards/matek/h743-mini/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/matek/h743-mini/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/matek/h743-mini/nuttx-config/scripts/script.ld b/boards/matek/h743-mini/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..85f4990724d5 --- /dev/null +++ b/boards/matek/h743-mini/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 = 1920K + + 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/matek/h743-mini/src/CMakeLists.txt b/boards/matek/h743-mini/src/CMakeLists.txt new file mode 100644 index 000000000000..f677ccf9e029 --- /dev/null +++ b/boards/matek/h743-mini/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# 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_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/matek/h743-mini/src/board_config.h b/boards/matek/h743-mini/src/board_config.h new file mode 100644 index 000000000000..151d6b59c5e5 --- /dev/null +++ b/boards/matek/h743-mini/src/board_config.h @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * 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_BLUE /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_nLED_GREEN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_ARMED_STATE_LED 1 // Green LED +#define BOARD_OVERLOAD_LED 0 // Blue LED + + +/* + * 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) + +/* 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, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PA7 */ GPIO_ADC12_INP7, \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* 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_BATTERY2_VOLTAGE_CHANNEL /* PA4 */ ADC1_CH(18) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA7 */ ADC1_CH(7) +#define ADC_AIRSPEED_IN_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_AIRSPEED_IN_CHANNEL) | \ + (1 << ADC_RSSI_IN_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 */ + + +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define GPIO_CAN1_SILENT_S0 /* PD3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN3) + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 12 +#define DIRECT_INPUT_TIMER_CHANNELS 12 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* Spare GPIO */ + +// #define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) +// #define GPIO_PD15 /* PD15 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15) +// #define GPIO_PG15 /* PG15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15) + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM_GPIO /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +// #define TONE_ALARM_TIMER 2 /* Timer 2 */ +// #define TONE_ALARM_CHANNEL 1 /* PA15 GPIO_TIM2_CH1OUT_2 */ +// #define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +// #define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + + +/* USB OTG FS + * + * PE2 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PE2 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTE|GPIO_PIN2) + + +/* 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 + +// #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) + + +/* 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_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nLED_BLUE, \ + GPIO_nLED_GREEN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#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/matek/h743-mini/src/bootloader_main.c b/boards/matek/h743-mini/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/matek/h743-mini/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/matek/h743-mini/src/hw_config.h b/boards/matek/h743-mini/src/hw_config.h new file mode 100644 index 000000000000..f289dc960d40 --- /dev/null +++ b/boards/matek/h743-mini/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,57600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 139 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 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/matek/h743-mini/src/i2c.cpp b/boards/matek/h743-mini/src/i2c.cpp new file mode 100644 index 000000000000..1444ea117204 --- /dev/null +++ b/boards/matek/h743-mini/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/matek/h743-mini/src/init.c b/boards/matek/h743-mini/src/init.c new file mode 100644 index 000000000000..bb1c6f975c09 --- /dev/null +++ b/boards/matek/h743-mini/src/init.c @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 + +__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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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 + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/matek/h743-mini/src/led.c b/boards/matek/h743-mini/src/led.c new file mode 100644 index 000000000000..2d60d89be979 --- /dev/null +++ b/boards/matek/h743-mini/src/led.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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_BLUE, // Indexed by LED_BLUE (defaulted to an output) + GPIO_nLED_GREEN +}; + +__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/matek/h743-mini/src/sdio.c b/boards/matek/h743-mini/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/matek/h743-mini/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/matek/h743-mini/src/spi.cpp b/boards/matek/h743-mini/src/spi.cpp new file mode 100644 index 000000000000..db673bedf0d3 --- /dev/null +++ b/boards/matek/h743-mini/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBusExternal(SPI::Bus::SPI3, { + initSPIConfigExternal(SPI::CS{GPIO::PortD, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/matek/h743-mini/src/timer_config.cpp b/boards/matek/h743-mini/src/timer_config.cpp new file mode 100644 index 000000000000..e862bf2a7a40 --- /dev/null +++ b/boards/matek/h743-mini/src/timer_config.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), + initIOTimer(Timer::Timer15), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), + 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}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + 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); + + + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +// #define CCER_C1_NUM_BITS 4 +// #define POLARITY(c) (GTIM_CCER_CC1P << (((c)-1) * CCER_C1_NUM_BITS)) +// #define DRIVE_TYPE(p) ((p)|GPIO_OPENDRAIN|GPIO_PULLUP) + +// static inline constexpr timer_io_channels_t initIOTimerChannelLED(const io_timers_t io_timers_conf[MAX_LED_TIMERS], +// Timer::TimerChannel timer, GPIO::GPIOPin pin, int ui_polarity) +// { +// timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, timer, pin); +// ret.gpio_out = DRIVE_TYPE(ret.gpio_out); +// ret.masks = POLARITY(ui_polarity); +// return ret; +// } + +// constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +// initIOTimerChannelLED(led_pwm_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}, 1), +// }; diff --git a/boards/matek/h743-mini/src/usb.c b/boards/matek/h743-mini/src/usb.c new file mode 100644 index 000000000000..4fdfafd25078 --- /dev/null +++ b/boards/matek/h743-mini/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_STM32F7_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/matek/h743-slim/bootloader.px4board b/boards/matek/h743-slim/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/matek/h743-slim/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board new file mode 100644 index 000000000000..9a6d9ed049d1 --- /dev/null +++ b/boards/matek/h743-slim/default.px4board @@ -0,0 +1,89 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_COMMON_LIGHT=n +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=n +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM=n +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=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 +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=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=n +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_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_GIMBAL=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=n +CONFIG_SYSTEMCMDS_MFT=n +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_MTD=n +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=n +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=n diff --git a/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin b/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin new file mode 100755 index 000000000000..bb6deba86efb Binary files /dev/null and b/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin differ diff --git a/boards/matek/h743-slim/firmware.prototype b/boards/matek/h743-slim/firmware.prototype new file mode 100644 index 000000000000..a1a9b1f7253d --- /dev/null +++ b/boards/matek/h743-slim/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1013, + "magic": "PX4FWv1", + "description": "Firmware for the MatekH743 board", + "image": "", + "build_time": 0, + "summary": "MatekH743", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/matek/h743-slim/init/rc.board_defaults b/boards/matek/h743-slim/init/rc.board_defaults new file mode 100644 index 000000000000..6d776625a130 --- /dev/null +++ b/boards/matek/h743-slim/init/rc.board_defaults @@ -0,0 +1,10 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 11 +param set-default BAT1_A_PER_V 40 + +param set-default BAT2_V_DIV 11 +param set-default BAT2_A_PER_V 40 diff --git a/boards/matek/h743-slim/init/rc.board_extras b/boards/matek/h743-slim/init/rc.board_extras new file mode 100644 index 000000000000..a39b81bdc223 --- /dev/null +++ b/boards/matek/h743-slim/init/rc.board_extras @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# if ! param compare OSD_ATXXXX_CFG 0 +# then +# atxxxx start -s +# fi + +atxxxx start -s + + +# DShot telemetry is always on UART7 +# dshot telemetry /dev/ttyS5 + diff --git a/boards/matek/h743-slim/init/rc.board_sensors b/boards/matek/h743-slim/init/rc.board_sensors new file mode 100644 index 000000000000..c37c3b3b5868 --- /dev/null +++ b/boards/matek/h743-slim/init/rc.board_sensors @@ -0,0 +1,22 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal SPI bus ICM-42605 +if ! icm42605 -R 14 -s start +then + # internal SPI bus ICM-20602 + icm20602 -R 12 -s start +fi + +# Internal SPI bus MPU-6000 +mpu6000 -R 12 -s start + +# Internal baro +dps310 -I start -a 118 + +# External mag +qmc5883l -X start -a 13 diff --git a/boards/matek/h743-slim/nuttx-config/Kconfig b/boards/matek/h743-slim/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/matek/h743-slim/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/matek/h743-slim/nuttx-config/bootloader/defconfig b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..92fb47fcec06 --- /dev/null +++ b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig @@ -0,0 +1,94 @@ +# +# 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/matek/h743-slim/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=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1013 +CONFIG_CDCACM_PRODUCTSTR="MatekH743" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1209 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=8 +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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART1=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_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/matek/h743-slim/nuttx-config/include/board.h b/boards/matek/h743-slim/nuttx-config/include/board.h new file mode 100644 index 000000000000..4bf2b6e45e0f --- /dev/null +++ b/boards/matek/h743-slim/nuttx-config/include/board.h @@ -0,0 +1,493 @@ +/************************************************************************************ + * 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_MATEKH743SLIM_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MATEKH743SLIM_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 MatekH743-Slim 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_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* 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_2 /* PD6 */ +#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_3 /* PB8 */ +#define GPIO_UART4_TX GPIO_UART4_TX_3 /* PB9 */ + +#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_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* 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 MPU6000 + * SPI2 is MAX7456 + * SPI3 is extern with PD4 and PE2 as CS + * SPI4 is ICM20602 + */ + +#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_3 /* PD7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(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 ADJ_SLEW_RATE(GPIO_SPI2_SCK_4) /* PB13 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_4 /* PB5 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PB3 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_1 /* PE14 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_1) /* PE12 */ + +/* 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_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) + +/* SDMMC1 + * + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + */ + +// #define GPIO_SDMMC1_D0 GPIO_SDMMC1_D0 /* PC8 */ +// #define GPIO_SDMMC1_D1 GPIO_SDMMC1_D1 /* PC9 */ +// #define GPIO_SDMMC1_D2 GPIO_SDMMC1_D2 /* PC10 */ +// #define GPIO_SDMMC1_D3 GPIO_SDMMC1_D3 /* PC11 */ +// #define GPIO_SDMMC1_CK GPIO_SDMMC1_CK /* PC12 */ +// #define GPIO_SDMMC1_CMD GPIO_SDMMC1_CMD /* PD2 */ + + +/* 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_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +# 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_MATEKH743SLIM_INCLUDE_BOARD_H */ diff --git a/boards/matek/h743-slim/nuttx-config/include/board_dma_map.h b/boards/matek/h743-slim/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..693132c389aa --- /dev/null +++ b/boards/matek/h743-slim/nuttx-config/include/board_dma_map.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + + +// DMAMUX2 +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2:83 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2:84 */ diff --git a/boards/matek/h743-slim/nuttx-config/nsh/defconfig b/boards/matek/h743-slim/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..8597eeb6afa2 --- /dev/null +++ b/boards/matek/h743-slim/nuttx-config/nsh/defconfig @@ -0,0 +1,228 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743-slim/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=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1013 +CONFIG_CDCACM_PRODUCTSTR="MatekH743" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1209 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_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_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_FLOWCONTROL_BROKEN=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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +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_SERIAL_CONSOLE=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/matek/h743-slim/nuttx-config/scripts/bootloader_script.ld b/boards/matek/h743-slim/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/matek/h743-slim/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/matek/h743-slim/nuttx-config/scripts/script.ld b/boards/matek/h743-slim/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c6a8ee1141fe --- /dev/null +++ b/boards/matek/h743-slim/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * 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 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 = 1920K + + 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/matek/h743-slim/src/CMakeLists.txt b/boards/matek/h743-slim/src/CMakeLists.txt new file mode 100644 index 000000000000..f677ccf9e029 --- /dev/null +++ b/boards/matek/h743-slim/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# 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_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/matek/h743-slim/src/board_config.h b/boards/matek/h743-slim/src/board_config.h new file mode 100644 index 000000000000..f977a2e8dd41 --- /dev/null +++ b/boards/matek/h743-slim/src/board_config.h @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * 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_BLUE /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_nLED_GREEN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_ARMED_STATE_LED 1 // Green LED +#define BOARD_OVERLOAD_LED 0 // Blue LED + + +/* + * 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) + +/* 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, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PA7 */ GPIO_ADC12_INP7, \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* 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_BATTERY2_VOLTAGE_CHANNEL /* PA4 */ ADC1_CH(18) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA7 */ ADC1_CH(7) +#define ADC_AIRSPEED_IN_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_AIRSPEED_IN_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL)) + +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define GPIO_CAN1_SILENT_S0 /* PD3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN3) + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 12 +#define DIRECT_INPUT_TIMER_CHANNELS 12 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* Spare GPIO */ + +#define GPIO_VIDEO_PWR /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN10) +#define GPIO_VIDEO_CAM /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN11) +// #define GPIO_PG15 /* PG15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15) + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM_GPIO /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +// #define TONE_ALARM_TIMER 2 /* Timer 2 */ +// #define TONE_ALARM_CHANNEL 1 /* PA15 GPIO_TIM2_CH1OUT_2 */ +// #define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +// #define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + + +/* USB OTG FS + * + * PE2 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PE2 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTE|GPIO_PIN2) + + +/* 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 + +// #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) + + +/* 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_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nLED_BLUE, \ + GPIO_nLED_GREEN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_VIDEO_PWR, \ + GPIO_VIDEO_CAM, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#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/matek/h743-slim/src/bootloader_main.c b/boards/matek/h743-slim/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/matek/h743-slim/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/matek/h743-slim/src/hw_config.h b/boards/matek/h743-slim/src/hw_config.h new file mode 100644 index 000000000000..3256b9c63d10 --- /dev/null +++ b/boards/matek/h743-slim/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,57600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1013 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 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/matek/h743-slim/src/i2c.cpp b/boards/matek/h743-slim/src/i2c.cpp new file mode 100644 index 000000000000..1444ea117204 --- /dev/null +++ b/boards/matek/h743-slim/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/matek/h743-slim/src/init.c b/boards/matek/h743-slim/src/init.c new file mode 100644 index 000000000000..f78e570c5ab5 --- /dev/null +++ b/boards/matek/h743-slim/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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); + } + +#endif + + +// #if defined(FLASH_BASED_PARAMS) +// static sector_descriptor_t params_sector_map[] = { +// {6, 128 * 1024, 0x081C0000}, +// {7, 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_BLUE); +// return -ENODEV; +// } + +// #endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/matek/h743-slim/src/led.c b/boards/matek/h743-slim/src/led.c new file mode 100644 index 000000000000..2d60d89be979 --- /dev/null +++ b/boards/matek/h743-slim/src/led.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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_BLUE, // Indexed by LED_BLUE (defaulted to an output) + GPIO_nLED_GREEN +}; + +__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/matek/h743-slim/src/sdio.c b/boards/matek/h743-slim/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/matek/h743-slim/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/matek/h743-slim/src/spi.cpp b/boards/matek/h743-slim/src/spi.cpp new file mode 100644 index 000000000000..db673bedf0d3 --- /dev/null +++ b/boards/matek/h743-slim/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBusExternal(SPI::Bus::SPI3, { + initSPIConfigExternal(SPI::CS{GPIO::PortD, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/matek/h743-slim/src/timer_config.cpp b/boards/matek/h743-slim/src/timer_config.cpp new file mode 100644 index 000000000000..e862bf2a7a40 --- /dev/null +++ b/boards/matek/h743-slim/src/timer_config.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), + initIOTimer(Timer::Timer15), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), + 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}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + 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); + + + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +// #define CCER_C1_NUM_BITS 4 +// #define POLARITY(c) (GTIM_CCER_CC1P << (((c)-1) * CCER_C1_NUM_BITS)) +// #define DRIVE_TYPE(p) ((p)|GPIO_OPENDRAIN|GPIO_PULLUP) + +// static inline constexpr timer_io_channels_t initIOTimerChannelLED(const io_timers_t io_timers_conf[MAX_LED_TIMERS], +// Timer::TimerChannel timer, GPIO::GPIOPin pin, int ui_polarity) +// { +// timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, timer, pin); +// ret.gpio_out = DRIVE_TYPE(ret.gpio_out); +// ret.masks = POLARITY(ui_polarity); +// return ret; +// } + +// constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +// initIOTimerChannelLED(led_pwm_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}, 1), +// }; diff --git a/boards/matek/h743-slim/src/usb.c b/boards/matek/h743-slim/src/usb.c new file mode 100644 index 000000000000..4fdfafd25078 --- /dev/null +++ b/boards/matek/h743-slim/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_STM32F7_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/matek/h743/bootloader.px4board b/boards/matek/h743/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/matek/h743/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board new file mode 100644 index 000000000000..9a6d9ed049d1 --- /dev/null +++ b/boards/matek/h743/default.px4board @@ -0,0 +1,89 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_COMMON_LIGHT=n +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=n +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPM=n +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=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 +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=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=n +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_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODULES_GIMBAL=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=n +CONFIG_SYSTEMCMDS_MFT=n +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_MTD=n +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=n +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=n diff --git a/boards/matek/h743/extras/matek_h743_bootloader.bin b/boards/matek/h743/extras/matek_h743_bootloader.bin new file mode 100755 index 000000000000..f863a2499ac7 Binary files /dev/null and b/boards/matek/h743/extras/matek_h743_bootloader.bin differ diff --git a/boards/matek/h743/firmware.prototype b/boards/matek/h743/firmware.prototype new file mode 100644 index 000000000000..a9177f689b7a --- /dev/null +++ b/boards/matek/h743/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1013, + "magic": "PX4FWv1", + "description": "Firmware for the MatekH743 boards", + "image": "", + "build_time": 0, + "summary": "MatekH743", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/matek/h743/init/rc.board_extras b/boards/matek/h743/init/rc.board_extras new file mode 100644 index 000000000000..a39b81bdc223 --- /dev/null +++ b/boards/matek/h743/init/rc.board_extras @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# if ! param compare OSD_ATXXXX_CFG 0 +# then +# atxxxx start -s +# fi + +atxxxx start -s + + +# DShot telemetry is always on UART7 +# dshot telemetry /dev/ttyS5 + diff --git a/boards/matek/h743/init/rc.board_sensors b/boards/matek/h743/init/rc.board_sensors new file mode 100644 index 000000000000..c37c3b3b5868 --- /dev/null +++ b/boards/matek/h743/init/rc.board_sensors @@ -0,0 +1,22 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal SPI bus ICM-42605 +if ! icm42605 -R 14 -s start +then + # internal SPI bus ICM-20602 + icm20602 -R 12 -s start +fi + +# Internal SPI bus MPU-6000 +mpu6000 -R 12 -s start + +# Internal baro +dps310 -I start -a 118 + +# External mag +qmc5883l -X start -a 13 diff --git a/boards/matek/h743/nuttx-config/Kconfig b/boards/matek/h743/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/matek/h743/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/matek/h743/nuttx-config/bootloader/defconfig b/boards/matek/h743/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..12722330d447 --- /dev/null +++ b/boards/matek/h743/nuttx-config/bootloader/defconfig @@ -0,0 +1,92 @@ +# +# 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/matek/h743/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=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART1=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_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/matek/h743/nuttx-config/include/board.h b/boards/matek/h743/nuttx-config/include/board.h new file mode 100644 index 000000000000..4bf2b6e45e0f --- /dev/null +++ b/boards/matek/h743/nuttx-config/include/board.h @@ -0,0 +1,493 @@ +/************************************************************************************ + * 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_MATEKH743SLIM_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MATEKH743SLIM_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 MatekH743-Slim 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_NLEDS 2 + +#define BOARD_LED_BLUE BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) + +/* 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_2 /* PD6 */ +#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_3 /* PB8 */ +#define GPIO_UART4_TX GPIO_UART4_TX_3 /* PB9 */ + +#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_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* 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 MPU6000 + * SPI2 is MAX7456 + * SPI3 is extern with PD4 and PE2 as CS + * SPI4 is ICM20602 + */ + +#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_3 /* PD7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(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 ADJ_SLEW_RATE(GPIO_SPI2_SCK_4) /* PB13 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_4 /* PB5 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PB3 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_1 /* PE14 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_1) /* PE12 */ + +/* 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_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) + +/* SDMMC1 + * + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + */ + +// #define GPIO_SDMMC1_D0 GPIO_SDMMC1_D0 /* PC8 */ +// #define GPIO_SDMMC1_D1 GPIO_SDMMC1_D1 /* PC9 */ +// #define GPIO_SDMMC1_D2 GPIO_SDMMC1_D2 /* PC10 */ +// #define GPIO_SDMMC1_D3 GPIO_SDMMC1_D3 /* PC11 */ +// #define GPIO_SDMMC1_CK GPIO_SDMMC1_CK /* PC12 */ +// #define GPIO_SDMMC1_CMD GPIO_SDMMC1_CMD /* PD2 */ + + +/* 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_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +# 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_MATEKH743SLIM_INCLUDE_BOARD_H */ diff --git a/boards/matek/h743/nuttx-config/include/board_dma_map.h b/boards/matek/h743/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..693132c389aa --- /dev/null +++ b/boards/matek/h743/nuttx-config/include/board_dma_map.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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 + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + + +// DMAMUX2 +#define DMAMAP_SPI4_RX DMAMAP_DMA12_SPI4RX_1 /* DMA2:83 */ +#define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2:84 */ diff --git a/boards/matek/h743/nuttx-config/nsh/defconfig b/boards/matek/h743/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..adfc3394b0c7 --- /dev/null +++ b/boards/matek/h743/nuttx-config/nsh/defconfig @@ -0,0 +1,225 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/matek/h743/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=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="MatekH743" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_CLOCK_MONOTONIC=y +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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_FLOWCONTROL_BROKEN=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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +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_SERIAL_CONSOLE=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/matek/h743/nuttx-config/scripts/bootloader_script.ld b/boards/matek/h743/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/matek/h743/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/matek/h743/nuttx-config/scripts/script.ld b/boards/matek/h743/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..c6a8ee1141fe --- /dev/null +++ b/boards/matek/h743/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * 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 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 = 1920K + + 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/matek/h743/src/CMakeLists.txt b/boards/matek/h743/src/CMakeLists.txt new file mode 100644 index 000000000000..f677ccf9e029 --- /dev/null +++ b/boards/matek/h743/src/CMakeLists.txt @@ -0,0 +1,67 @@ +############################################################################ +# +# 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_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/matek/h743/src/board_config.h b/boards/matek/h743/src/board_config.h new file mode 100644 index 000000000000..151d6b59c5e5 --- /dev/null +++ b/boards/matek/h743/src/board_config.h @@ -0,0 +1,237 @@ +/**************************************************************************** + * + * 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_BLUE /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_nLED_GREEN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_ARMED_STATE_LED 1 // Green LED +#define BOARD_OVERLOAD_LED 0 // Blue LED + + +/* + * 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) + +/* 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, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PA7 */ GPIO_ADC12_INP7, \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* 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_BATTERY2_VOLTAGE_CHANNEL /* PA4 */ ADC1_CH(18) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA7 */ ADC1_CH(7) +#define ADC_AIRSPEED_IN_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_RSSI_IN_CHANNEL /* PC5 */ ADC1_CH(8) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_AIRSPEED_IN_CHANNEL) | \ + (1 << ADC_RSSI_IN_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 */ + + +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define GPIO_CAN1_SILENT_S0 /* PD3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN3) + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 12 +#define DIRECT_INPUT_TIMER_CHANNELS 12 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* Spare GPIO */ + +// #define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) +// #define GPIO_PD15 /* PD15 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15) +// #define GPIO_PG15 /* PG15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15) + + +/* Tone alarm output */ + +#define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM_GPIO /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +// #define TONE_ALARM_TIMER 2 /* Timer 2 */ +// #define TONE_ALARM_CHANNEL 1 /* PA15 GPIO_TIM2_CH1OUT_2 */ +// #define GPIO_TONE_ALARM_IDLE /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +// #define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + + +/* USB OTG FS + * + * PE2 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PE2 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTE|GPIO_PIN2) + + +/* 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 + +// #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) + + +/* 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_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nLED_BLUE, \ + GPIO_nLED_GREEN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#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/matek/h743/src/bootloader_main.c b/boards/matek/h743/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/matek/h743/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/matek/h743/src/hw_config.h b/boards/matek/h743/src/hw_config.h new file mode 100644 index 000000000000..f289dc960d40 --- /dev/null +++ b/boards/matek/h743/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,57600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 139 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 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/matek/h743/src/i2c.cpp b/boards/matek/h743/src/i2c.cpp new file mode 100644 index 000000000000..1444ea117204 --- /dev/null +++ b/boards/matek/h743/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/matek/h743/src/init.c b/boards/matek/h743/src/init.c new file mode 100644 index 000000000000..a2a531882cf7 --- /dev/null +++ b/boards/matek/h743/src/init.c @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 + +__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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/matek/h743/src/led.c b/boards/matek/h743/src/led.c new file mode 100644 index 000000000000..2d60d89be979 --- /dev/null +++ b/boards/matek/h743/src/led.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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_BLUE, // Indexed by LED_BLUE (defaulted to an output) + GPIO_nLED_GREEN +}; + +__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/matek/h743/src/sdio.c b/boards/matek/h743/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/matek/h743/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/matek/h743/src/spi.cpp b/boards/matek/h743/src/spi.cpp new file mode 100644 index 000000000000..db673bedf0d3 --- /dev/null +++ b/boards/matek/h743/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBusExternal(SPI::Bus::SPI3, { + initSPIConfigExternal(SPI::CS{GPIO::PortD, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42605, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortE, GPIO::Pin11}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/matek/h743/src/timer_config.cpp b/boards/matek/h743/src/timer_config.cpp new file mode 100644 index 000000000000..e862bf2a7a40 --- /dev/null +++ b/boards/matek/h743/src/timer_config.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), + initIOTimer(Timer::Timer15), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), + 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}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + 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); + + + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +// #define CCER_C1_NUM_BITS 4 +// #define POLARITY(c) (GTIM_CCER_CC1P << (((c)-1) * CCER_C1_NUM_BITS)) +// #define DRIVE_TYPE(p) ((p)|GPIO_OPENDRAIN|GPIO_PULLUP) + +// static inline constexpr timer_io_channels_t initIOTimerChannelLED(const io_timers_t io_timers_conf[MAX_LED_TIMERS], +// Timer::TimerChannel timer, GPIO::GPIOPin pin, int ui_polarity) +// { +// timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, timer, pin); +// ret.gpio_out = DRIVE_TYPE(ret.gpio_out); +// ret.masks = POLARITY(ui_polarity); +// return ret; +// } + +// constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +// initIOTimerChannelLED(led_pwm_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}, 1), +// }; diff --git a/boards/matek/h743/src/usb.c b/boards/matek/h743/src/usb.c new file mode 100644 index 000000000000..4fdfafd25078 --- /dev/null +++ b/boards/matek/h743/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_STM32F7_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/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake deleted file mode 100644 index 11e27489be8a..000000000000 --- a/boards/modalai/fc-v1/default.cmake +++ /dev/null @@ -1,131 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR modalai - MODEL fc-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS0 # UART1 / J10 - TEL1:/dev/ttyS6 # UART7 / J5 - TEL2:/dev/ttyS4 # UART5 / J1 - TEL3:/dev/ttyS1 # USART2 / J4 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm42688p - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - power_monitor/voxlpm - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - #tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board new file mode 100644 index 000000000000..fca6849ecabf --- /dev/null +++ b/boards/modalai/fc-v1/default.px4board @@ -0,0 +1,108 @@ +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_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/modalai/fc-v1/bootloader/modalai_fc-v1_bootloader.bin b/boards/modalai/fc-v1/extras/modalai_fc-v1_bootloader.bin similarity index 100% rename from boards/modalai/fc-v1/bootloader/modalai_fc-v1_bootloader.bin rename to boards/modalai/fc-v1/extras/modalai_fc-v1_bootloader.bin diff --git a/boards/modalai/fc-v1/init/rc.board_defaults b/boards/modalai/fc-v1/init/rc.board_defaults index d78d536d2572..e8abaa09b354 100644 --- a/boards/modalai/fc-v1/init/rc.board_defaults +++ b/boards/modalai/fc-v1/init/rc.board_defaults @@ -18,88 +18,10 @@ # Common settings across Flight Core configurations # -# Disable safety switch by default (pull high to 3.3V to enable) -# V106 - J13 pin 5 -# V110 - J1011 pin 5 -param set-default CBRK_IO_SAFETY 22027 - -# -# Stand Alone configuration -# -if ver hwtypecmp V106 -then - echo "Configuring Flight Core - V106" - - # - # In Flight Core, J1 can be setup to be used as a serial port for TELEM2 - # and connected to VOXL via cables. We'll configure this out of the box. - # The user can later change this if they want, as these are configurable - # and not necessarily required to be used with VOXL. - # - if [ $AUTOCNF = no ] - then - if param compare MAV_1_CONFIG 0 - then - echo "V106 - Defualt configuration TELEM2 on /dev/ttyS4 at 921600 in Normal Mode" - # TELEM2 - param set-default MAV_1_CONFIG 102 - # Onboard - param set-default MAV_1_MODE 2 - # VIO data - param set-default SER_TEL2_BAUD 921600 - fi - fi - - # User is setting defaults, so let's do it! - if [ $AUTOCNF = yes ] - then - echo "V106 - Auto Configuring TELEM2 on /dev/ttyS4 at 921600 in Normal Mode" - # TELEM2 - param set-default MAV_1_CONFIG 102 - # Onboard - param set-default MAV_1_MODE 2 - # VIO data - param set-default SER_TEL2_BAUD 921600 - fi -fi - -# -# VOXL-Flight configuration -# -if ver hwtypecmp V110 -then - echo "Configuring VOXL-Flight - V110" - - # - # TELEM2 port is physically routed in the PCB, thus not configurable. - # The following will detect a fresh install, or if the user has changed the setting and - # revert to the VOXL-Flight defaults. This does allow the user to change the mode and - # baud rates and mode if they choose to do so, although VOXL is expecting what is set below - # - if [ $AUTOCNF = no ] - then - if ! param compare MAV_1_CONFIG 102 - then - echo "V110 - Default configuration TELEM2 on /dev/ttyS4 at 921600 in Normal Mode" - # TELEM2 - param set-default MAV_1_CONFIG 102 - # Onboard - param set-default MAV_1_MODE 2 - param set-default SER_TEL2_BAUD 921600 - fi - fi - - # User is setting defaults, so let's do it! - if [ $AUTOCNF = yes ] - then - echo "V110 - Auto Configuring TELEM2 on /dev/ttyS4 at 921600 in Normal Mode" - # TELEM2 - param set-default MAV_1_CONFIG 102 - # Onboard - param set-default MAV_1_MODE 2 - param set-default SER_TEL2_BAUD 921600 - fi -fi +# TELEM2 +param set-default MAV_1_CONFIG 102 +param set-default MAV_1_MODE 2 +param set-default SER_TEL2_BAUD 921600 safety_button start diff --git a/boards/modalai/fc-v1/init/rc.board_mavlink b/boards/modalai/fc-v1/init/rc.board_mavlink deleted file mode 100644 index b3c20f583525..000000000000 --- a/boards/modalai/fc-v1/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# ModalAI FC-v1 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig index 02b0b7ff0caf..1c6174376b49 100644 --- a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig +++ b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/fc-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0xa32f CONFIG_CDCACM_PRODUCTSTR="PX4 FMU ModalAI FCv1" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -94,18 +97,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -115,16 +113,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -138,8 +138,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/modalai/fc-v1/rtps.px4board b/boards/modalai/fc-v1/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/modalai/fc-v1/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index 6db8e25798cf..11e46d658621 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -150,7 +150,6 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 #define GPIO_CAN1_SILENT /* PI11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN11) @@ -184,6 +183,7 @@ /* RC Serial port */ #define RC_SERIAL_PORT "/dev/ttyS5" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT /* Safety Switch: Enable the FMU to control it as there is no px4io in ModalAI FC-v1 */ #define GPIO_SAFETY_SWITCH_IN /* PF3 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTF|GPIO_PIN3) @@ -235,7 +235,6 @@ #define BOARD_ADC_BRICK_VALID (1) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -263,7 +262,6 @@ #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/modalai/fc-v1/src/init.c b/boards/modalai/fc-v1/src/init.c index 75bef2628a5d..beee10b8754e 100644 --- a/boards/modalai/fc-v1/src/init.c +++ b/boards/modalai/fc-v1/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -255,23 +256,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -288,7 +277,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/modalai/fc-v1/src/manifest.c b/boards/modalai/fc-v1/src/manifest.c index f6d5a960c3ab..7b2266bd1fa5 100644 --- a/boards/modalai/fc-v1/src/manifest.c +++ b/boards/modalai/fc-v1/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -123,7 +125,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/modalai/fc-v1/src/timer_config.cpp b/boards/modalai/fc-v1/src/timer_config.cpp index 6c3e0de9b7a5..322f1cb9c309 100644 --- a/boards/modalai/fc-v1/src/timer_config.cpp +++ b/boards/modalai/fc-v1/src/timer_config.cpp @@ -47,7 +47,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer1, DMA{DMA::Index2, DMA::Stream5, DMA::Channel6}), - initIOTimer(Timer::Timer4), + initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), }; diff --git a/boards/modalai/fc-v2/bootloader.px4board b/boards/modalai/fc-v2/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/modalai/fc-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/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board new file mode 100644 index 000000000000..5f8fdb69f36c --- /dev/null +++ b/boards/modalai/fc-v2/default.px4board @@ -0,0 +1,100 @@ +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_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin b/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin new file mode 100755 index 000000000000..ad47bb2f1d70 Binary files /dev/null and b/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin differ diff --git a/boards/modalai/fc-v2/firmware.prototype b/boards/modalai/fc-v2/firmware.prototype new file mode 100644 index 000000000000..7cd48b5c590a --- /dev/null +++ b/boards/modalai/fc-v2/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 41776, + "magic": "PX4FWv1", + "description": "Firmware for the ModalAI Flight Core v2 board", + "image": "", + "build_time": 0, + "summary": "modalaifcv2", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/modalai/fc-v2/init/rc.board_defaults b/boards/modalai/fc-v2/init/rc.board_defaults new file mode 100644 index 000000000000..e68faa0f4043 --- /dev/null +++ b/boards/modalai/fc-v2/init/rc.board_defaults @@ -0,0 +1,4 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ diff --git a/boards/modalai/fc-v2/init/rc.board_sensors b/boards/modalai/fc-v2/init/rc.board_sensors new file mode 100644 index 000000000000..da0a0a8a7915 --- /dev/null +++ b/boards/modalai/fc-v2/init/rc.board_sensors @@ -0,0 +1,28 @@ +#!/bin/sh +# +# ModalAI FC-v2 specific board sensors init +#------------------------------------------------------------------------------ + +# VOXL ESC - TODO +#if param greater UART_ESC_CONFIG 0 +#then +# modalai_esc -d /dev/ttyS4 start +#fi + +board_adc start + +# VOXL Power Module +voxlpm -X -b 3 -k -T VBATT start +voxlpm -X -b 3 -T P5VDC start + +# Internal SPI1 ICM-42688 +icm42688p -s -b 1 -R 12 start + +# Internal SPI2 ICM-42688 +icm42688p -s -b 2 -R 12 start + +# Internal I2C mag +bmm150 -I start + +# Internal I2C baro +bmp388 -I start diff --git a/boards/modalai/fc-v2/nuttx-config/Kconfig b/boards/modalai/fc-v2/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/modalai/fc-v2/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/modalai/fc-v2/nuttx-config/bootloader/defconfig b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..4d847c27132f --- /dev/null +++ b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig @@ -0,0 +1,93 @@ +# +# 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/modalai/fc-v2/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H753II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0xa330 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL ModalAI FCv2" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x0483 +CONFIG_CDCACM_VENDORSTR="ModalAI" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=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_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/modalai/fc-v2/nuttx-config/include/board.h b/boards/modalai/fc-v2/nuttx-config/include/board.h new file mode 100644 index 000000000000..34a8d2c2af74 --- /dev/null +++ b/boards/modalai/fc-v2/nuttx-config/include/board.h @@ -0,0 +1,555 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6x/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_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) * 60 = 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(60) +#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) * 60) +#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 = 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 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 PX4 FMUV6X 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_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ + +#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 */ +// GPIO_UART5_CTS No remap /* 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 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* 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 IMU1 + * SPI2 is IMU2 + * SPI3 is Not Used + * SPI4 is Magnetometer + * SPI5 is FRAM + * 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_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_6) /* PI1 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE14 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_2) /* PE12 */ + +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_2 /* PH7 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_1 /* PF11 */ +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ + +#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_I2C3_SCL GPIO_I2C3_SCL_1 /* PA8 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8) + +#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 */ + +/* The STM32 H7 connects to a TI DP83848TSQ/NOPB + * using RMII + * + * STM32 H7 BOARD DP83848TSQ/NOPB + * GPIO SIGNAL PIN NAME + * -------- ------------ ------------- + * PA7 ETH_CRS_DV CRS_DV + * PC1 ETH_MDC MDC + * PA2 ETH_MDIO MDIO + * PA1 ETH_REF_CL X1 + * PC4 ETH_RXD0 RX_D0 + * PC5 ETH_RXD1 RX_D1 + * PB11 ETH_TX_EN TX_EN + * PG13 ETH_TXD0 TX_D0 + * PG12 ETH_TXD1 TX_D1 + * + * The PHY address is 1, since COL/PHYAD0 features a pull up. + */ + +#define GPIO_ETH_RMII_TX_EN GPIO_ETH_RMII_TX_EN_1 /* PB11 */ +#define GPIO_ETH_RMII_TXD0 GPIO_ETH_RMII_TXD0_2 /* PG13 */ +#define GPIO_ETH_RMII_TXD1 GPIO_ETH_RMII_TXD1_2 /* PG12 */ + + +/* 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_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +# 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/modalai/fc-v2/nuttx-config/include/board_dma_map.h b/boards/modalai/fc-v2/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..768597447439 --- /dev/null +++ b/boards/modalai/fc-v2/nuttx-config/include/board_dma_map.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* DMA1:72 */ + +// DMAMUX2 +#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_1 /* DMA2:61 */ +#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_1 /* DMA2:62 */ + +#define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* BDMA:11 */ +#define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* BDMA:12 */ diff --git a/boards/modalai/fc-v2/nuttx-config/nsh/defconfig b/boards/modalai/fc-v2/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..1e46ecd9963e --- /dev/null +++ b/boards/modalai/fc-v2/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_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/modalai/fc-v2/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H753II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0xa330 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU ModalAI FCv2" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x0483 +CONFIG_CDCACM_VENDORSTR="ModalAI" +CONFIG_CLOCK_MONOTONIC=y +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_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_I2C3=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_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=3000 +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_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/modalai/fc-v2/nuttx-config/scripts/bootloader_script.ld b/boards/modalai/fc-v2/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..5cc69e66f11b --- /dev/null +++ b/boards/modalai/fc-v2/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * 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 ModalAi FlightCore v2 uses an STM32H756II 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 STM32H756II, 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 ModalAi FlightCore v2 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 STM32H756II 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/modalai/fc-v2/nuttx-config/scripts/script.ld b/boards/modalai/fc-v2/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..b31de772cead --- /dev/null +++ b/boards/modalai/fc-v2/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * 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 ModalAi FlightCore v2 uses an STM32H756II 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 STM32H756II, 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 ModalAi FlightCore v2 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 STM32H756II 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 = 1920K + + 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/modalai/fc-v2/src/CMakeLists.txt b/boards/modalai/fc-v2/src/CMakeLists.txt new file mode 100644 index 000000000000..7ff9cdf5f504 --- /dev/null +++ b/boards/modalai/fc-v2/src/CMakeLists.txt @@ -0,0 +1,70 @@ +############################################################################ +# +# 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_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + 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 + manifest.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_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/modalai/fc-v2/src/board_config.h b/boards/modalai/fc-v2/src/board_config.h new file mode 100644 index 000000000000..26f22e4b732c --- /dev/null +++ b/boards/modalai/fc-v2/src/board_config.h @@ -0,0 +1,386 @@ +/**************************************************************************** + * + * Copyright (c) 2016, 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 board_config.h + * + * ModalAI FC v2 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +#define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* Configuration ************************************************************************************/ + + +/* 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 PC12 - UART5_TX_TELEM2 - Trace Connector Pin 8 + + */ +#undef TRACE_PINS + +/* 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 +# if defined(CONFIG_STM32H7_UART5) && (GPIO_UART5_TX == GPIO_UART5_TX_3) +# error Need to disable CONFIG_STM32H7_UART5 for Trace 3 (Retarget to CAN2 if need be) +# endif +#endif + + +/* SPI */ + +#define GPIO_SPI4_MAG_INT /* PD12 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN12) + +#define GPIO_SYNC /* PE9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_100MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) + +/* I2C busses */ + +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_BMP388 0x76 + +#define GPIO_I2C4_DRDY1_BMP388 /* PG5 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5) + +/* + * 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 \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC2 */ GPIO_ADC123_INP12, \ + /* PC3 */ GPIO_ADC12_INP13, \ + /* PH3 */ GPIO_ADC3_INP14, \ + /* PH4 */ GPIO_ADC3_INP15 + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_SCALED_V5_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_ADC3_6V6_CHANNEL /* PC2 */ ADC3_CH(12) +#define ADC_ADC3_3V3_CHANNEL /* PC3 */ ADC3_CH(13) +#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_SCALED_V5_CHANNEL) | \ + (1 << ADC_ADC3_6V6_CHANNEL) | \ + (1 << ADC_ADC3_3V3_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_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 {'V','2','x', 'x',0} +#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ +#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ + +/* PE6 is nARMED --> FCv2 this goes to TP13 + * 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 + */ +#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_ARMED_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nVDD_USB_VALID /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) /* Low for USB power, High for DC power */ + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) + +/* Spare GPIO */ + +#define CAN1_SILENT /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) + +/* Future Use - IMU FSYNC */ + +#define IMU_FYSNC_TIMER 14 /* Timer 14 */ +#define IMU_FYSNC_CHANNEL 1 /* PF9 GPIO_TIM14_CH1OUT_2 */ + +#define GPIO_IMU_FYSNC_1 /* PF9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN9) + +#define GPIO_IMU_FYSNC_IDLE GPIO_IMU_FYSNC_1 +#define GPIO_IMU_FYSNC GPIO_TIM14_CH1OUT_2 + +/* 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) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +//#define HRT_PPM_CHANNEL /* T8C1 */ 1 /* use capture/compare channel 1 */ +//#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2 + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS5" + +/* Input Capture Channels. */ +#define INPUT_CAP1_TIMER 1 +#define INPUT_CAP1_CHANNEL /* T1C2 */ 2 +#define GPIO_INPUT_CAP1 /* PE11 */ GPIO_TIM1_CH2IN + +/* 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 + +/* Power switch controls ******************************************************/ + +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * ModalAI FC v2 has NO separate RC_IN + * + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART + */ + +#define SPEKTRUM_RX_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(SPEKTRUM_RX_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(SPEKTRUM_RX_AS_OUT, (_one_true)) + +#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_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_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)) +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* ModalAI FC v2 never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#if !defined(BOARD_HAS_LTC44XX_VALIDS) || BOARD_HAS_LTC44XX_VALIDS == 0 +# define BOARD_ADC_BRICK1_VALID (1) +# define BOARD_ADC_BRICK2_VALID (0) +#elif BOARD_HAS_LTC44XX_VALIDS == 1 +# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +# define BOARD_ADC_BRICK2_VALID (0) +#elif BOARD_HAS_LTC44XX_VALIDS == 2 +# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) +#elif BOARD_HAS_LTC44XX_VALIDS == 3 +# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) +# define BOARD_ADC_BRICK3_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK3_VALID)) +#elif BOARD_HAS_LTC44XX_VALIDS == 4 +# define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +# define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) +# define BOARD_ADC_BRICK3_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK3_VALID)) +# define BOARD_ADC_BRICK4_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK4_VALID)) +#else +# error Unsupported BOARD_HAS_LTC44XX_VALIDS value +#endif + + +/* 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_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + CAN1_SILENT, \ + GPIO_SYNC, \ + GPIO_IMU_FYSNC_IDLE, \ + GPIO_IMU_FYSNC, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#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 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/modalai/fc-v2/src/bootloader_main.c b/boards/modalai/fc-v2/src/bootloader_main.c new file mode 100644 index 000000000000..cb8426165268 --- /dev/null +++ b/boards/modalai/fc-v2/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 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/modalai/fc-v2/src/can.c b/boards/modalai/fc-v2/src/can.c new file mode 100644 index 000000000000..fb8f730d4b8b --- /dev/null +++ b/boards/modalai/fc-v2/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.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 + +#endif /* CONFIG_CAN */ diff --git a/boards/modalai/fc-v2/src/hw_config.h b/boards/modalai/fc-v2/src/hw_config.h new file mode 100644 index 000000000000..474a55c0fda4 --- /dev/null +++ b/boards/modalai/fc-v2/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * 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,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 41776 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 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/modalai/fc-v2/src/i2c.cpp b/boards/modalai/fc-v2/src/i2c.cpp new file mode 100644 index 000000000000..56f17e3a2d05 --- /dev/null +++ b/boards/modalai/fc-v2/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), // External GPS/Mag, suggested + initI2CBusExternal(2), // External Aviator + initI2CBusExternal(3), // External VOXL PM + initI2CBusInternal(4), // Internal Barometer, Internal Mag +}; diff --git a/boards/modalai/fc-v2/src/init.c b/boards/modalai/fc-v2/src/init.c new file mode 100644 index 000000000000..dbc6ec4637d6 --- /dev/null +++ b/boards/modalai/fc-v2/src/init.c @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 + * + * PX4FMU-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 +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * 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 */ + board_control_spi_sensors_power(false, 0xffff); + + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* 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 */ + VDD_3V3_SPEKTRUM_POWER_EN(last); + board_control_spi_sensors_power(true, 0xffff); + +} + +/************************************************************************************ + * 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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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 SPI interfaces (we can do this here as long as we only have a single SPI hw config version - + * otherwise we need to move this after board_determine_hw_info()) */ + _Static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support"); + + 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; 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 */ + board_spi_reset(10, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + + 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 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); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/modalai/fc-v2/src/led.c b/boards/modalai/fc-v2/src/led.c new file mode 100644 index 000000000000..bff4b098640f --- /dev/null +++ b/boards/modalai/fc-v2/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU 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 + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#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/modalai/fc-v2/src/manifest.c b/boards/modalai/fc-v2/src/manifest.c new file mode 100644 index 000000000000..4f01dd360f30 --- /dev/null +++ b/boards/modalai/fc-v2/src/manifest.c @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +// M0079 +static const px4_hw_mft_item_t hw_mft_list_fc0200[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { + {0x0300, hw_mft_list_fc0200, arraySize(hw_mft_list_fc0200)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/modalai/fc-v2/src/mtd.cpp b/boards/modalai/fc-v2/src/mtd.cpp new file mode 100644 index 000000000000..765ce93e2d1b --- /dev/null +++ b/boards/modalai/fc-v2/src/mtd.cpp @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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 +#include +// KiB BS nB +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi5, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &fmum_fram + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/modalai/fc-v2/src/sdio.c b/boards/modalai/fc-v2/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/modalai/fc-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/modalai/fc-v2/src/spi.cpp b/boards/modalai/fc-v2/src/spi.cpp new file mode 100644 index 000000000000..69555c852653 --- /dev/null +++ b/boards/modalai/fc-v2/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 +#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::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_MAG_DEVTYPE_BMM150, SPI::CS{GPIO::PortH, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/modalai/fc-v2/src/timer_config.cpp b/boards/modalai/fc-v2/src/timer_config.cpp new file mode 100644 index 000000000000..6df6c8dc1688 --- /dev/null +++ b/boards/modalai/fc-v2/src/timer_config.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 + +/* Timer allocation + * + * TIM5_CH4 T FMU_CH1 + * TIM5_CH3 T FMU_CH2 + * TIM5_CH2 T FMU_CH3 + * TIM5_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * + * TIM12_CH1 T FMU_CH7 + * TIM12_CH2 T FMU_CH8 + * + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer12), + initIOTimer(Timer::Timer1), + initIOTimer(Timer::Timer2), +}; + +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::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/modalai/fc-v2/src/usb.c b/boards/modalai/fc-v2/src/usb.c new file mode 100644 index 000000000000..9d5915c0e6d1 --- /dev/null +++ b/boards/modalai/fc-v2/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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#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 PX4FMU 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/mro/ctrl-zero-classic/bootloader.px4board b/boards/mro/ctrl-zero-classic/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board new file mode 100644 index 000000000000..3915a6afb02b --- /dev/null +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -0,0 +1,105 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin b/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin new file mode 100755 index 000000000000..2b83862c2ec4 Binary files /dev/null and b/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-classic/firmware.prototype b/boards/mro/ctrl-zero-classic/firmware.prototype new file mode 100644 index 000000000000..cc8499694136 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1022, + "magic": "mRo-ctrl-zero-classic", + "description": "Firmware for the mRo-ctrl-zero-classic board", + "image": "", + "build_time": 0, + "summary": "mRo-ctrl-zero-classic", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_defaults b/boards/mro/ctrl-zero-classic/init/rc.board_defaults new file mode 100644 index 000000000000..32117f4a5886 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/init/rc.board_defaults @@ -0,0 +1,9 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_A_PER_V 17 + +safety_button start diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_sensors b/boards/mro/ctrl-zero-classic/init/rc.board_sensors new file mode 100644 index 000000000000..111e3eb0440b --- /dev/null +++ b/boards/mro/ctrl-zero-classic/init/rc.board_sensors @@ -0,0 +1,18 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal ICM-20602 +icm20602 -s -b 1 -R 8 start + +# Internal SPI bus BMI088 accel & gyro +bmi088 -A -s -b 5 -R 8 start +bmi088 -G -s -b 5 -R 8 start + +# Internal ICM-20948 (with magnetometer) +icm20948 -s -b 1 -R 8 -M start + +# Interal DPS310 (barometer) +dps310 -s -b 2 start diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..d29195ef5820 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig @@ -0,0 +1,94 @@ +# +# 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/mro/ctrl-zero-classic/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1022 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZero Classic" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_FS_PROCFS_MAX_TASKS=8 +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_SDCLONE_DISABLE=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_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=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_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h new file mode 100644 index 000000000000..9b1ae6342d33 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -0,0 +1,280 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2020 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. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,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_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 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(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#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 / 2) * 80) +#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(32) +#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) * 32) +#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(32) +#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) * 32) +#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 + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 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 */ +#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 + + +/* UART/USART */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ + +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ + +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ + +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ + +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#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_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board_dma_map.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..10011ed3719c --- /dev/null +++ b/boards/mro/ctrl-zero-classic/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 + +// DMAMUX1 +#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_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..7de8c128fdce --- /dev/null +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -0,0 +1,236 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1022 +CONFIG_CDCACM_PRODUCTSTR="mRoControlZero Classic" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" +CONFIG_CLOCK_MONOTONIC=y +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_DISABLE_MQUEUE=y +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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_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_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=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_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_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=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_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_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +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_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/scripts/bootloader_script.ld b/boards/mro/ctrl-zero-classic/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..3fb4cc1f33ce --- /dev/null +++ b/boards/mro/ctrl-zero-classic/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * 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 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 = 0x08000000, LENGTH = 128K + 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(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_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) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-classic/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..02e763a790fb --- /dev/null +++ b/boards/mro/ctrl-zero-classic/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 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 STM32H743XIH6 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 = 1920K + + 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/mro/ctrl-zero-classic/src/CMakeLists.txt b/boards/mro/ctrl-zero-classic/src/CMakeLists.txt new file mode 100644 index 000000000000..822841a74a49 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/CMakeLists.txt @@ -0,0 +1,64 @@ +############################################################################ +# +# 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 + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/mro/ctrl-zero-classic/src/board_config.h b/boards/mro/ctrl-zero-classic/src/board_config.h new file mode 100644 index 000000000000..dfb0dfcb1a78 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/board_config.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * 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 +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_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) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 12 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* 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 */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* 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)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* 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 BOARD_HAS_STATIC_MANIFEST 1 + + +#define BOARD_NUM_IO_TIMERS 5 + + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN2_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/mro/ctrl-zero-classic/src/bootloader_main.c b/boards/mro/ctrl-zero-classic/src/bootloader_main.c new file mode 100644 index 000000000000..bb6b4dc23aad --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 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_configgpio(GPIO_OTGFS_VBUS); +} + +__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/mro/ctrl-zero-classic/src/hw_config.h b/boards/mro/ctrl-zero-classic/src/hw_config.h new file mode 100644 index 000000000000..8fdaec9e7e41 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#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 1024 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#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/mro/ctrl-zero-classic/src/i2c.cpp b/boards/mro/ctrl-zero-classic/src/i2c.cpp new file mode 100644 index 000000000000..1b8927c69939 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/mro/ctrl-zero-classic/src/init.c b/boards/mro/ctrl-zero-classic/src/init.c new file mode 100644 index 000000000000..daaace48fb2f --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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 initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +__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) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* 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 */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * 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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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 pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * 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) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* 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_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/mro/ctrl-zero-classic/src/led.c b/boards/mro/ctrl-zero-classic/src/led.c new file mode 100644 index 000000000000..0e7beb3a9e3b --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__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(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/mro/ctrl-zero-classic/src/spi.cpp b/boards/mro/ctrl-zero-classic/src/spi.cpp new file mode 100644 index 000000000000..4a4c3502bbd9 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/mro/ctrl-zero-classic/src/timer_config.cpp b/boards/mro/ctrl-zero-classic/src/timer_config.cpp new file mode 100644 index 000000000000..23bb7a1decc7 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/timer_config.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1}), + initIOTimer(Timer::Timer15), //DMA{DMA::Index2}), + initIOTimer(Timer::Timer8, 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::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortC, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/mro/ctrl-zero-classic/src/usb.c b/boards/mro/ctrl-zero-classic/src/usb.c new file mode 100644 index 000000000000..e60a93bb59b4 --- /dev/null +++ b/boards/mro/ctrl-zero-classic/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * 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_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 +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake deleted file mode 100644 index 90193cf71aca..000000000000 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ /dev/null @@ -1,136 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-f7-oem - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - #RC:/dev/ttyS3 - TEL3:/dev/ttyS4 - #FRSKY:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/dps310 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board new file mode 100644 index 000000000000..e0ee41bb869b --- /dev/null +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -0,0 +1,106 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-f7-oem/init/rc.board_defaults b/boards/mro/ctrl-zero-f7-oem/init/rc.board_defaults index 9c542a77300e..32117f4a5886 100644 --- a/boards/mro/ctrl-zero-f7-oem/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-f7-oem/init/rc.board_defaults @@ -3,4 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_A_PER_V 17 + safety_button start diff --git a/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink b/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink deleted file mode 100644 index 9a4a80a6d3e5..000000000000 --- a/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# mRo Control Zero specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig index abf7771a993a..ad56d733ab0d 100644 --- a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-f7-oem/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x008D CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroF7" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -94,18 +97,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -115,16 +113,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -139,8 +139,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -212,8 +211,8 @@ CONFIG_UART7_RXBUFSIZE=600 CONFIG_UART7_TXBUFSIZE=1500 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_TXBUFSIZE=1500 CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y CONFIG_USART2_OFLOWCONTROL=y diff --git a/boards/mro/ctrl-zero-f7-oem/src/board_config.h b/boards/mro/ctrl-zero-f7-oem/src/board_config.h index 1d86505b72e0..5f00959de047 100644 --- a/boards/mro/ctrl-zero-f7-oem/src/board_config.h +++ b/boards/mro/ctrl-zero-f7-oem/src/board_config.h @@ -95,10 +95,6 @@ (1 << ADC_SCALED_V5_CHANNEL) | \ (1 << ADC_RC_RSSI_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V */ -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -108,7 +104,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Power supply control and monitoring GPIOs */ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -178,7 +173,6 @@ #define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ #define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/mro/ctrl-zero-f7-oem/src/init.c b/boards/mro/ctrl-zero-f7-oem/src/init.c index 4d8e58d159b7..0b0b07ff124f 100644 --- a/boards/mro/ctrl-zero-f7-oem/src/init.c +++ b/boards/mro/ctrl-zero-f7-oem/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -204,23 +205,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -237,7 +226,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake deleted file mode 100644 index ec1abbd7ada9..000000000000 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ /dev/null @@ -1,136 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-f7 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - #RC:/dev/ttyS3 - #CONSOLE:/dev/ttyS4 - #FRSKY:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/dps310 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board new file mode 100644 index 000000000000..955849347816 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -0,0 +1,106 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_defaults b/boards/mro/ctrl-zero-f7/init/rc.board_defaults index 9c542a77300e..32117f4a5886 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-f7/init/rc.board_defaults @@ -3,4 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_A_PER_V 17 + safety_button start diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_mavlink b/boards/mro/ctrl-zero-f7/init/rc.board_mavlink deleted file mode 100644 index 9a4a80a6d3e5..000000000000 --- a/boards/mro/ctrl-zero-f7/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# mRo Control Zero specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig index a0015c3dc829..ad7a96047da6 100644 --- a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-f7/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x008D CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroF7" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -94,18 +97,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -115,16 +113,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -139,8 +139,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h index c0bd06592ae2..7dc7ce045555 100644 --- a/boards/mro/ctrl-zero-f7/src/board_config.h +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -95,10 +95,6 @@ (1 << ADC_SCALED_V5_CHANNEL) | \ (1 << ADC_RC_RSSI_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V */ -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -107,7 +103,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Power supply control and monitoring GPIOs */ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -177,7 +172,6 @@ #define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ #define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c index 4d8e58d159b7..0b0b07ff124f 100644 --- a/boards/mro/ctrl-zero-f7/src/init.c +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -204,23 +205,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -237,7 +226,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/mro/ctrl-zero-h7-oem/bootloader.cmake b/boards/mro/ctrl-zero-h7-oem/bootloader.cmake deleted file mode 100644 index d4543ee775a9..000000000000 --- a/boards/mro/ctrl-zero-h7-oem/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-h7-oem - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/mro/ctrl-zero-h7-oem/bootloader.px4board b/boards/mro/ctrl-zero-h7-oem/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/mro/ctrl-zero-h7-oem/default.cmake b/boards/mro/ctrl-zero-h7-oem/default.cmake deleted file mode 100644 index 3fd744d53a83..000000000000 --- a/boards/mro/ctrl-zero-h7-oem/default.cmake +++ /dev/null @@ -1,132 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-h7-oem - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - UAVCAN_INTERFACES 2 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - #RC:/dev/ttyS3 - TEL3:/dev/ttyS4 - #FRSKY:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/dps310 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board new file mode 100644 index 000000000000..3915a6afb02b --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -0,0 +1,105 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin b/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin new file mode 100755 index 000000000000..86dc50de7b9c Binary files /dev/null and b/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults b/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults index 0e211f353952..32117f4a5886 100644 --- a/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults @@ -3,11 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ -param set-default BAT_V_DIV 10.1 param set-default BAT1_V_DIV 10.1 - -param set-default BAT_A_PER_V 24 -param set-default BAT1_A_PER_V 24 - +param set-default BAT1_A_PER_V 17 safety_button start diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig index faf0f8608ecb..920a30bf1deb 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7-oem/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,7 +29,8 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x1023 +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1024 CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZeroH7 OEM" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 @@ -51,10 +52,9 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 +CONFIG_FS_PROCFS_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +83,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h index 2e7a6a031fd4..9b1ae6342d33 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -222,34 +222,35 @@ /* UART/USART */ -#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ -#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ -#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ -#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ -#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ -#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ -#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ -#define GPIO_UART4_RX GPIO_UART4_RX_1 /* PA1 */ -#define GPIO_UART4_TX GPIO_UART4_TX_1 /* PA0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ -#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ -#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ -#define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ -#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ -#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ -#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ /* CAN */ -#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ -#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ -#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ -#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ /* SPI */ @@ -265,7 +266,7 @@ #define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ #define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ -#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_1 /* PF9 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ /* I2C */ diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig index 932842c9009b..ac21595e4759 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7-oem/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1024 CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7 OEM" CONFIG_CDCACM_RXBUFSIZE=600 @@ -72,6 +75,7 @@ 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 @@ -84,7 +88,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -97,18 +100,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -118,13 +116,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -143,8 +141,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -188,7 +185,6 @@ CONFIG_STM32H7_SPI5=y CONFIG_STM32H7_SPI5_DMA=y CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y @@ -208,7 +204,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld index 50f6307bc58f..02e763a790fb 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld @@ -187,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); @@ -207,7 +212,6 @@ SECTIONS _sram4_heap_start = ABSOLUTE(.); } > SRAM4 - /* Stabs debugging sections. */ .stab 0 : { *(.stab) } .stabstr 0 : { *(.stabstr) } @@ -221,13 +225,4 @@ SECTIONS .debug_line 0 : { *(.debug_line) } .debug_pubnames 0 : { *(.debug_pubnames) } .debug_aranges 0 : { *(.debug_aranges) } - - .ramfunc : { - _sramfuncs = .; - *(.ramfunc .ramfunc.*) - . = ALIGN(4); - _eramfuncs = .; - } > ITCM_RAM AT > FLASH - - _framfuncs = LOADADDR(.ramfunc); } diff --git a/boards/mro/ctrl-zero-h7-oem/rtps.px4board b/boards/mro/ctrl-zero-h7-oem/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt index 8e734c6fca9c..822841a74a49 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt +++ b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/mro/ctrl-zero-h7-oem/src/board_config.h b/boards/mro/ctrl-zero-h7-oem/src/board_config.h index 6538ff0d8a6b..c573b6522e78 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/board_config.h +++ b/boards/mro/ctrl-zero-h7-oem/src/board_config.h @@ -77,11 +77,9 @@ /* CAN Silence: Silent mode control */ #define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) -#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Power supply control and monitoring GPIOs */ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -156,11 +154,9 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_ENABLE_CONSOLE_BUFFER @@ -171,7 +167,6 @@ GPIO_CAN2_TX, \ GPIO_CAN2_RX, \ GPIO_CAN1_SILENT_S0, \ - GPIO_CAN2_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_TONE_ALARM_IDLE, \ diff --git a/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c b/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c +++ b/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/mro/ctrl-zero-h7-oem/src/init.c b/boards/mro/ctrl-zero-h7-oem/src/init.c index 0c8f4cf1573c..daaace48fb2f 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/init.c +++ b/boards/mro/ctrl-zero-h7-oem/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -182,12 +186,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/mro/ctrl-zero-h7/bootloader.cmake b/boards/mro/ctrl-zero-h7/bootloader.cmake deleted file mode 100644 index 0679706aa24a..000000000000 --- a/boards/mro/ctrl-zero-h7/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-h7 - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/mro/ctrl-zero-h7/bootloader.px4board b/boards/mro/ctrl-zero-h7/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/mro/ctrl-zero-h7/default.cmake b/boards/mro/ctrl-zero-h7/default.cmake deleted file mode 100644 index 9cf2c0eb0168..000000000000 --- a/boards/mro/ctrl-zero-h7/default.cmake +++ /dev/null @@ -1,132 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL ctrl-zero-h7 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - UAVCAN_INTERFACES 1 - SERIAL_PORTS - TEL1:/dev/ttyS0 - TEL2:/dev/ttyS1 - GPS1:/dev/ttyS2 - #RC:/dev/ttyS3 - TEL3:/dev/ttyS4 - #FRSKY:/dev/ttyS5 - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/dps310 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board new file mode 100644 index 000000000000..3a631809d228 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -0,0 +1,106 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin b/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin new file mode 100755 index 000000000000..6c09ebda1557 Binary files /dev/null and b/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_defaults b/boards/mro/ctrl-zero-h7/init/rc.board_defaults index 0e211f353952..32117f4a5886 100644 --- a/boards/mro/ctrl-zero-h7/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-h7/init/rc.board_defaults @@ -3,11 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ -param set-default BAT_V_DIV 10.1 param set-default BAT1_V_DIV 10.1 - -param set-default BAT_A_PER_V 24 -param set-default BAT1_A_PER_V 24 - +param set-default BAT1_A_PER_V 17 safety_button start diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/ctrl-zero-h7/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig index 271291fd9597..bea411e71f16 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1023 CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZeroH7" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,9 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 +CONFIG_FS_PROCFS_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +83,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h index 7dbde2d7d410..78a0048760b6 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -221,32 +221,32 @@ /* UART/USART */ -#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ -#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ -#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ -#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ -#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ -#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ -#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ -#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ -#define GPIO_UART4_RX GPIO_UART4_RX_1 /* PA1 */ -#define GPIO_UART4_TX GPIO_UART4_TX_1 /* PA0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ -#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ -#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ -#define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ -#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ -#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ -#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ /* CAN */ -#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ -#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ /* SPI */ @@ -262,7 +262,7 @@ #define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ #define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ -#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_1 /* PF9 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ /* I2C */ diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig index 24c1c3f98ee9..1db30f975ad7 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-h7/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1023 CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7" CONFIG_CDCACM_RXBUFSIZE=600 @@ -72,6 +75,7 @@ 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 @@ -84,7 +88,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -97,18 +100,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -118,13 +116,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -143,8 +141,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -185,7 +182,6 @@ CONFIG_STM32H7_SPI5=y CONFIG_STM32H7_SPI5_DMA=y CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y @@ -205,7 +201,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld index 076ff3dcee06..02e763a790fb 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld +++ b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld @@ -187,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); @@ -199,6 +204,7 @@ SECTIONS } > AXI_SRAM /* Emit the the D3 power domain section for locating BDMA data */ + .sram4_reserve (NOLOAD) : { *(.sram4) @@ -206,7 +212,6 @@ SECTIONS _sram4_heap_start = ABSOLUTE(.); } > SRAM4 - /* Stabs debugging sections. */ .stab 0 : { *(.stab) } .stabstr 0 : { *(.stabstr) } @@ -220,13 +225,4 @@ SECTIONS .debug_line 0 : { *(.debug_line) } .debug_pubnames 0 : { *(.debug_pubnames) } .debug_aranges 0 : { *(.debug_aranges) } - - .ramfunc : { - _sramfuncs = .; - *(.ramfunc .ramfunc.*) - . = ALIGN(4); - _eramfuncs = .; - } > ITCM_RAM AT > FLASH - - _framfuncs = LOADADDR(.ramfunc); } diff --git a/boards/mro/ctrl-zero-h7/rtps.px4board b/boards/mro/ctrl-zero-h7/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/ctrl-zero-h7/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt index 8e734c6fca9c..822841a74a49 100644 --- a/boards/mro/ctrl-zero-h7/src/CMakeLists.txt +++ b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/mro/ctrl-zero-h7/src/board_config.h b/boards/mro/ctrl-zero-h7/src/board_config.h index c38b6796aa5c..cd8f5921efff 100644 --- a/boards/mro/ctrl-zero-h7/src/board_config.h +++ b/boards/mro/ctrl-zero-h7/src/board_config.h @@ -80,7 +80,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Power supply control and monitoring GPIOs */ #define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -155,11 +154,9 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/mro/ctrl-zero-h7/src/bootloader_main.c b/boards/mro/ctrl-zero-h7/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/mro/ctrl-zero-h7/src/bootloader_main.c +++ b/boards/mro/ctrl-zero-h7/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/mro/ctrl-zero-h7/src/init.c b/boards/mro/ctrl-zero-h7/src/init.c index 0c8f4cf1573c..daaace48fb2f 100644 --- a/boards/mro/ctrl-zero-h7/src/init.c +++ b/boards/mro/ctrl-zero-h7/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -182,12 +186,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/mro/pixracerpro/bootloader.cmake b/boards/mro/pixracerpro/bootloader.cmake deleted file mode 100644 index dc38de86b8e1..000000000000 --- a/boards/mro/pixracerpro/bootloader.cmake +++ /dev/null @@ -1,9 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL pixracerpro - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 -) diff --git a/boards/mro/pixracerpro/bootloader.px4board b/boards/mro/pixracerpro/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/mro/pixracerpro/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake deleted file mode 100644 index 9546250857f2..000000000000 --- a/boards/mro/pixracerpro/default.cmake +++ /dev/null @@ -1,133 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL pixracerpro - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - UAVCAN_INTERFACES 2 - SERIAL_PORTS - #SPARE:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - GPS1:/dev/ttyS3 - #RC:/dev/ttyS4 - #CONSOLE:/dev/ttyS5 - #FRSKY:/dev/ttyS6 - DRIVERS - adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/dps310 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20948 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board new file mode 100644 index 000000000000..4163c6c8087d --- /dev/null +++ b/boards/mro/pixracerpro/default.px4board @@ -0,0 +1,104 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin b/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin new file mode 100755 index 000000000000..78dffd233157 Binary files /dev/null and b/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin differ diff --git a/boards/mro/pixracerpro/init/rc.board_defaults b/boards/mro/pixracerpro/init/rc.board_defaults index 6f1b54fa0253..9c0d8e44c1c1 100644 --- a/boards/mro/pixracerpro/init/rc.board_defaults +++ b/boards/mro/pixracerpro/init/rc.board_defaults @@ -3,8 +3,5 @@ # board specific defaults #------------------------------------------------------------------------------ -param set-default BAT_V_DIV 10.1 param set-default BAT1_V_DIV 10.1 - -param set-default BAT_A_PER_V 17 param set-default BAT1_A_PER_V 17 diff --git a/boards/mro/pixracerpro/init/rc.board_mavlink b/boards/mro/pixracerpro/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/pixracerpro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig index 380fc3f10c56..0f4a2da8fc43 100644 --- a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/pixracerpro/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1017 CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo Pixracer Pro" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,9 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 +CONFIG_FS_PROCFS_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +83,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig index 5ecc9462bc30..6ecff1756a49 100644 --- a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/pixracerpro/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x1017 CONFIG_CDCACM_PRODUCTSTR="mRoPixracerPro" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +98,13 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -116,13 +114,13 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -141,8 +139,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -187,7 +184,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y @@ -206,7 +202,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/mro/pixracerpro/nuttx-config/scripts/script.ld b/boards/mro/pixracerpro/nuttx-config/scripts/script.ld index 68343681c066..5102235444c0 100644 --- a/boards/mro/pixracerpro/nuttx-config/scripts/script.ld +++ b/boards/mro/pixracerpro/nuttx-config/scripts/script.ld @@ -111,6 +111,7 @@ MEMORY { ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + 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 */ @@ -186,7 +187,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > AXI_SRAM AT > FLASH + + /* 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(.); diff --git a/boards/mro/pixracerpro/rtps.px4board b/boards/mro/pixracerpro/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/mro/pixracerpro/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/mro/pixracerpro/src/CMakeLists.txt b/boards/mro/pixracerpro/src/CMakeLists.txt index 0c034edc6b1d..eacffd570277 100644 --- a/boards/mro/pixracerpro/src/CMakeLists.txt +++ b/boards/mro/pixracerpro/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/mro/pixracerpro/src/board_config.h b/boards/mro/pixracerpro/src/board_config.h index 797e0a4953ea..0d067bb12cb9 100644 --- a/boards/mro/pixracerpro/src/board_config.h +++ b/boards/mro/pixracerpro/src/board_config.h @@ -84,12 +84,11 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Power supply control and monitoring GPIOs */ -#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_POWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) -#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_VDD_BRICK1_VALID GPIO_POWER_IN_A /* Brick 1 Is Chosen */ #define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) @@ -119,7 +118,7 @@ /* RC Serial port */ #define RC_SERIAL_PORT "/dev/ttyS4" -#define RC_SERIAL_SINGLEWIRE +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT #define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) @@ -144,11 +143,9 @@ */ #define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) #define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED -#define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; #define BOARD_DMA_ALLOC_POOL_SIZE 5120 /* This board provides a DMA pool and APIs */ #define BOARD_HAS_ON_RESET 1 /* This board provides the board_on_reset interface */ #define BOARD_ENABLE_CONSOLE_BUFFER @@ -163,7 +160,7 @@ GPIO_CAN2_SILENT_S0, \ GPIO_LEVEL_SHIFTER_OE, \ GPIO_PWM_VOLT_SEL, \ - GPIO_nPOWER_IN_A, \ + GPIO_POWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D0), \ PX4_GPIO_PIN_OFF(GPIO_SDMMC1_D1), \ diff --git a/boards/mro/pixracerpro/src/bootloader_main.c b/boards/mro/pixracerpro/src/bootloader_main.c index 0a481dcc19e1..bb6b4dc23aad 100644 --- a/boards/mro/pixracerpro/src/bootloader_main.c +++ b/boards/mro/pixracerpro/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/mro/pixracerpro/src/init.c b/boards/mro/pixracerpro/src/init.c index c9107bd4ff80..7e80ea7cd048 100644 --- a/boards/mro/pixracerpro/src/init.c +++ b/boards/mro/pixracerpro/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -58,6 +60,8 @@ #include #include +#include + __BEGIN_DECLS extern void led_init(void); extern void led_on(int led); @@ -181,12 +185,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio_dev) { syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); - return ERROR; } if (mmcsd_slotinitialize(0, sdio_dev) != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); - return ERROR; } /* Assume that the SD card is inserted. What choice do we have? */ diff --git a/boards/mro/pixracerpro/src/usb.c b/boards/mro/pixracerpro/src/usb.c index 360d84d6c846..3d86aca28017 100644 --- a/boards/mro/pixracerpro/src/usb.c +++ b/boards/mro/pixracerpro/src/usb.c @@ -41,6 +41,7 @@ #include #include #include +#include /************************************************************************************ * Name: stm32_usbsuspend diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake deleted file mode 100644 index 291613356421..000000000000 --- a/boards/mro/x21-777/default.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL x21-777 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - # IO DEBUG:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - GPS1:/dev/ttyS3 - # PX4IO:/dev/ttyS4 - # CONSOLE:/dev/ttyS5 - # OSD:/dev/tty6 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #imu # all available imu drivers - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board new file mode 100644 index 000000000000..df2ea1627bec --- /dev/null +++ b/boards/mro/x21-777/default.px4board @@ -0,0 +1,107 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21-777/init/rc.board_defaults b/boards/mro/x21-777/init/rc.board_defaults new file mode 100644 index 000000000000..5d576dd74711 --- /dev/null +++ b/boards/mro/x21-777/init/rc.board_defaults @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 diff --git a/boards/mro/x21-777/init/rc.board_mavlink b/boards/mro/x21-777/init/rc.board_mavlink deleted file mode 100644 index 08fed10e71ec..000000000000 --- a/boards/mro/x21-777/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/x21-777/nuttx-config/include/board.h b/boards/mro/x21-777/nuttx-config/include/board.h index fc1bb5de88a7..04cc6c1acd62 100644 --- a/boards/mro/x21-777/nuttx-config/include/board.h +++ b/boards/mro/x21-777/nuttx-config/include/board.h @@ -281,5 +281,5 @@ #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 -#define GPIO_SPI2_SCK GPIO_SPI2_SCK_2 +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_3 diff --git a/boards/mro/x21-777/nuttx-config/nsh/defconfig b/boards/mro/x21-777/nuttx-config/nsh/defconfig index d4bf10cadb03..1ddbbe7600c3 100644 --- a/boards/mro/x21-777/nuttx-config/nsh/defconfig +++ b/boards/mro/x21-777/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/x21-777/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0088 CONFIG_CDCACM_PRODUCTSTR="mRo x2.1-777" CONFIG_CDCACM_RXBUFSIZE=600 @@ -70,6 +73,7 @@ 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 @@ -82,7 +86,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -94,18 +97,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -115,16 +113,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -139,8 +139,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -199,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART4_RXBUFSIZE=1200 +CONFIG_UART4_RXDMA=n +CONFIG_UART4_TXBUFSIZE=1200 CONFIG_UART7_BAUD=57600 CONFIG_UART7_RXBUFSIZE=600 CONFIG_UART7_SERIAL_CONSOLE=y @@ -226,10 +225,10 @@ CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_RXDMA=y CONFIG_USART3_TXBUFSIZE=3000 CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=1500 -CONFIG_USART6_TXDMA=y +CONFIG_USART6_RXBUFSIZE=1200 +CONFIG_USART6_RXDMA=n +CONFIG_USART6_TXBUFSIZE=1200 +CONFIG_USART6_TXDMA=n CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/mro/x21-777/src/board_config.h b/boards/mro/x21-777/src/board_config.h index 62a44f759773..30abf1e4678c 100644 --- a/boards/mro/x21-777/src/board_config.h +++ b/boards/mro/x21-777/src/board_config.h @@ -91,7 +91,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* USB OTG FS * @@ -118,7 +117,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/mro/x21-777/src/init.c b/boards/mro/x21-777/src/init.c index 43946b3ff207..3f5f00e3775e 100644 --- a/boards/mro/x21-777/src/init.c +++ b/boards/mro/x21-777/src/init.c @@ -43,6 +43,8 @@ #include "board_config.h" +#include + #include #include #include @@ -193,22 +195,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #if defined(SERIAL_HAVE_RXDMA) - /* set up the serial DMA polling */ + // 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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* initial LED state */ @@ -224,7 +213,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake deleted file mode 100644 index 86b3b23322c4..000000000000 --- a/boards/mro/x21/default.cmake +++ /dev/null @@ -1,136 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR mro - MODEL x21 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - # IO DEBUG:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - GPS1:/dev/ttyS3 - # PX4IO:/dev/ttyS4 - # CONSOLE:/dev/ttyS5 - # OSD:/dev/tty6 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #imu # all available imu drivers - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board new file mode 100644 index 000000000000..a5b400901108 --- /dev/null +++ b/boards/mro/x21/default.px4board @@ -0,0 +1,107 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/x21/bootloader/mro_x21_bootloader.bin b/boards/mro/x21/extras/mro_x21_bootloader.bin similarity index 100% rename from boards/mro/x21/bootloader/mro_x21_bootloader.bin rename to boards/mro/x21/extras/mro_x21_bootloader.bin diff --git a/boards/mro/x21/init/rc.board_defaults b/boards/mro/x21/init/rc.board_defaults new file mode 100644 index 000000000000..5d576dd74711 --- /dev/null +++ b/boards/mro/x21/init/rc.board_defaults @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 diff --git a/boards/mro/x21/init/rc.board_mavlink b/boards/mro/x21/init/rc.board_mavlink deleted file mode 100644 index a005e687686c..000000000000 --- a/boards/mro/x21/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# mRo x21 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/x21/nuttx-config/nsh/defconfig b/boards/mro/x21/nuttx-config/nsh/defconfig index d68e06785185..fe47ead119d7 100644 --- a/boards/mro/x21/nuttx-config/nsh/defconfig +++ b/boards/mro/x21/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/x21/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -34,6 +35,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -41,6 +43,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0021 CONFIG_CDCACM_PRODUCTSTR="PX4 AUAV X2.1" CONFIG_CDCACM_RXBUFSIZE=600 @@ -90,18 +93,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,7 +115,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -134,8 +131,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -175,7 +171,7 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI_DMA=y CONFIG_STM32_SPI_DMATHRESHOLD=8 diff --git a/boards/mro/x21/src/board_config.h b/boards/mro/x21/src/board_config.h index 605ec66c8b42..be6da1a33209 100644 --- a/boards/mro/x21/src/board_config.h +++ b/boards/mro/x21/src/board_config.h @@ -102,7 +102,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* USB OTG FS * @@ -129,7 +128,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/mro/x21/src/init.c b/boards/mro/x21/src/init.c index 40e54add37ab..107312832bb7 100644 --- a/boards/mro/x21/src/init.c +++ b/boards/mro/x21/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -240,22 +241,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -272,7 +262,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ @@ -288,7 +277,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) @@ -303,14 +291,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ - sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); led_on(LED_AMBER); - return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ @@ -319,7 +304,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); led_on(LED_AMBER); - return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ @@ -328,7 +312,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif /* Configure the HW based on the manifest */ - px4_platform_configure(); return OK; diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake deleted file mode 100644 index 2182ed226a23..000000000000 --- a/boards/nxp/fmuk66-e/default.cmake +++ /dev/null @@ -1,131 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-e - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm42688p - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm # NOT Portable YET - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board new file mode 100644 index 000000000000..463ae96cdfb1 --- /dev/null +++ b/boards/nxp/fmuk66-e/default.px4board @@ -0,0 +1,101 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BAROMETER_MPL3115A2=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/nxp/fmuk66-e/bootloader/nxp_fmuk66-e_bootloader.bin b/boards/nxp/fmuk66-e/extras/nxp_fmuk66-e_bootloader.bin similarity index 100% rename from boards/nxp/fmuk66-e/bootloader/nxp_fmuk66-e_bootloader.bin rename to boards/nxp/fmuk66-e/extras/nxp_fmuk66-e_bootloader.bin diff --git a/boards/nxp/fmuk66-e/init/rc.board_defaults b/boards/nxp/fmuk66-e/init/rc.board_defaults index 3b99b63eb3e3..eed00b78ecbe 100644 --- a/boards/nxp/fmuk66-e/init/rc.board_defaults +++ b/boards/nxp/fmuk66-e/init/rc.board_defaults @@ -3,5 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 + rgbled_pwm start safety_button start diff --git a/boards/nxp/fmuk66-e/init/rc.board_mavlink b/boards/nxp/fmuk66-e/init/rc.board_mavlink deleted file mode 100644 index fbe9af9a8bf7..000000000000 --- a/boards/nxp/fmuk66-e/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# NXP fmuk66-e specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/nxp/fmuk66-e/init/rc.board_sensors b/boards/nxp/fmuk66-e/init/rc.board_sensors index 518c5a35e9df..d47d01ced70c 100644 --- a/boards/nxp/fmuk66-e/init/rc.board_sensors +++ b/boards/nxp/fmuk66-e/init/rc.board_sensors @@ -16,4 +16,4 @@ bmi088 -A -R 8 -s start bmi088 -G -R 8 -s start # Internal SPI bus ICM-42688 -#icm42688p -R 12 -s start +icm42688p -R 12 -s start diff --git a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig index 4d3167c360d4..9c28b0789631 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig @@ -6,12 +6,13 @@ # modifications. # # CONFIG_DISABLE_OS_API is not set +# CONFIG_KINETIS_EDMA_HOE is not set # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_SPI is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmuk66-e/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="kinetis" @@ -65,7 +66,11 @@ CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y CONFIG_KINETIS_CRC=y -CONFIG_KINETIS_DMA=y +CONFIG_KINETIS_EDMA=y +CONFIG_KINETIS_EDMA_ELINK=y +CONFIG_KINETIS_EDMA_ERCA=y +CONFIG_KINETIS_EDMA_ERGA=y +CONFIG_KINETIS_EDMA_NTCD=16 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y CONFIG_KINETIS_ENET=y CONFIG_KINETIS_FLEXCAN0=y @@ -75,6 +80,7 @@ CONFIG_KINETIS_I2C0=y CONFIG_KINETIS_I2C1=y CONFIG_KINETIS_LPTMR0=y CONFIG_KINETIS_LPUART0=y +CONFIG_KINETIS_LPUART0_RXDMA=y CONFIG_KINETIS_MERGE_TTY=y CONFIG_KINETIS_PDB=y CONFIG_KINETIS_PIT=y @@ -88,7 +94,9 @@ CONFIG_KINETIS_SDHC=y CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y CONFIG_KINETIS_SPI0=y CONFIG_KINETIS_SPI1=y +CONFIG_KINETIS_SPI1_DMA=y CONFIG_KINETIS_SPI2=y +CONFIG_KINETIS_SPI_DMA=y CONFIG_KINETIS_UART0=y CONFIG_KINETIS_UART0_RXDMA=y CONFIG_KINETIS_UART1=y @@ -132,9 +140,7 @@ CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_WRITE_BUFFERS=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -142,11 +148,8 @@ CONFIG_NSH_DISABLE_DIRNAME=y CONFIG_NSH_DISABLE_EXPORT=y CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -170,7 +173,6 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 CONFIG_RAW_BINARY=y @@ -185,8 +187,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig index 5bc4634a5772..063557447b60 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig @@ -6,12 +6,13 @@ # modifications. # # CONFIG_DISABLE_OS_API is not set +# CONFIG_KINETIS_EDMA_HOE is not set # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_SPI is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmuk66-e/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="kinetis" @@ -26,6 +27,7 @@ CONFIG_BOARD_LOOPSPERMSEC=15175 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y +CONFIG_CAN_CONNS=1 CONFIG_CDCACM=y CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_PRODUCTID=0x001c @@ -40,8 +42,6 @@ CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_ETH0_PHY_TJA1100=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -63,11 +63,15 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_IOB_NBUFFERS=48 -CONFIG_IOB_NCHAINS=16 +CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y CONFIG_KINETIS_CRC=y -CONFIG_KINETIS_DMA=y +CONFIG_KINETIS_EDMA=y +CONFIG_KINETIS_EDMA_ELINK=y +CONFIG_KINETIS_EDMA_ERCA=y +CONFIG_KINETIS_EDMA_ERGA=y +CONFIG_KINETIS_EDMA_NTCD=16 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y CONFIG_KINETIS_ENET=y CONFIG_KINETIS_FLEXCAN0=y @@ -77,6 +81,7 @@ CONFIG_KINETIS_I2C0=y CONFIG_KINETIS_I2C1=y CONFIG_KINETIS_LPTMR0=y CONFIG_KINETIS_LPUART0=y +CONFIG_KINETIS_LPUART0_RXDMA=y CONFIG_KINETIS_MERGE_TTY=y CONFIG_KINETIS_PDB=y CONFIG_KINETIS_PIT=y @@ -90,7 +95,9 @@ CONFIG_KINETIS_SDHC=y CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y CONFIG_KINETIS_SPI0=y CONFIG_KINETIS_SPI1=y +CONFIG_KINETIS_SPI1_DMA=y CONFIG_KINETIS_SPI2=y +CONFIG_KINETIS_SPI_DMA=y CONFIG_KINETIS_UART0=y CONFIG_KINETIS_UART0_RXDMA=y CONFIG_KINETIS_UART1=y @@ -112,6 +119,7 @@ CONFIG_LPUART0_SERIAL_CONSOLE=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y CONFIG_MMCSD_SDIO=y CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y @@ -123,6 +131,7 @@ CONFIG_NETUTILS_TELNETD=y CONFIG_NET_BROADCAST=y CONFIG_NET_CAN=y CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_FILTER_MAX=0 CONFIG_NET_CAN_RAW_TX_DEADLINE=y CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ICMP=y @@ -130,9 +139,7 @@ CONFIG_NET_ICMP_SOCKET=y CONFIG_NET_TCP=y CONFIG_NET_TIMESTAMP=y CONFIG_NET_UDP=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -141,11 +148,8 @@ CONFIG_NSH_DISABLE_DIRNAME=y CONFIG_NSH_DISABLE_EXPORT=y CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -169,7 +173,6 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 CONFIG_RAW_BINARY=y @@ -184,8 +187,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/nxp/fmuk66-e/rtps.cmake b/boards/nxp/fmuk66-e/rtps.cmake deleted file mode 100644 index 06ddac7182e0..000000000000 --- a/boards/nxp/fmuk66-e/rtps.cmake +++ /dev/null @@ -1,131 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-e - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm42688p - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm # NOT Portable YET - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-e/rtps.px4board b/boards/nxp/fmuk66-e/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/nxp/fmuk66-e/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake deleted file mode 100644 index 7ede05dd029f..000000000000 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ /dev/null @@ -1,131 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-e - LABEL socketcan - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm42688p - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm # NOT Portable YET - tone_alarm - #uavcannode_v1 - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-e/socketcan.px4board b/boards/nxp/fmuk66-e/socketcan.px4board new file mode 100644 index 000000000000..7463620eb99b --- /dev/null +++ b/boards/nxp/fmuk66-e/socketcan.px4board @@ -0,0 +1,4 @@ +CONFIG_DRIVERS_UAVCAN=n +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/boards/nxp/fmuk66-e/src/board_config.h b/boards/nxp/fmuk66-e/src/board_config.h index f3beb66dd62f..4961977bc66d 100644 --- a/boards/nxp/fmuk66-e/src/board_config.h +++ b/boards/nxp/fmuk66-e/src/board_config.h @@ -249,12 +249,6 @@ __END_DECLS /* PTB6 ADC1_SE12 */ ADC1_GPIO(12), \ /* PTB7 ADC1_SE13 */ ADC1_GPIO(13) - - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - - /* User GPIOs * */ @@ -270,7 +264,6 @@ __END_DECLS // todo:Design this! #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 #define GPIO_ULTRASOUND_TRIGGER /* PTD0 */ (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTD | PIN0) #define GPIO_ULTRASOUND_ECHO /* PTA10 */ (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTA | PIN10) @@ -330,7 +323,6 @@ __END_DECLS #define BOARD_ADC_PERIPH_5V_OC (0) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_HAS_LED_PWM 1 @@ -339,7 +331,7 @@ __END_DECLS #define LED_TIM3_CH4OUT /* PTC8 RGB_B */ PIN_FTM3_CH4_1 /* This board provides a DMA pool and APIs */ -#define BOARD_DMA_ALLOC_POOL_SIZE 5120 +#define BOARD_DMA_ALLOC_POOL_SIZE 2048 /* This board provides the board_on_reset interface */ diff --git a/boards/nxp/fmuk66-e/src/init.c b/boards/nxp/fmuk66-e/src/init.c index 37563bdf9d07..ecb7f1bd8703 100644 --- a/boards/nxp/fmuk66-e/src/init.c +++ b/boards/nxp/fmuk66-e/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,7 @@ #include #include -#include -#include +#include #include "board_config.h" #include "arm_arch.h" @@ -188,6 +188,26 @@ kinetis_boardinitialize(void) VDD_3V3_SPEKTRUM_POWER_EN(true); } +/**************************************************************************** + * Name: kinetis_serial_dma_poll_all + * + * Description: + * Checks receive DMA buffers for received bytes that have not accumulated + * to the point where the DMA half/full interrupt has triggered. + * + * This function should be called from a timer or other periodic context. + * + ****************************************************************************/ + +void kinetis_lpserial_dma_poll_all(void) +{ +#if defined(LPSERIAL_HAVE_DMA) + kinetis_lpserial_dma_poll(); +#endif +#if defined(SERIAL_HAVE_DMA) + kinetis_serial_dma_poll(); +#endif +} /**************************************************************************** * Name: board_app_initialize @@ -234,24 +254,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA +#if defined(SERIAL_HAVE_DMA) || defined(LPSERIAL_HAVE_DMA) + // 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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)kinetis_serial_dma_poll, - NULL); -#endif + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)kinetis_lpserial_dma_poll_all, NULL); +#endif /* SERIAL_HAVE_DMA || LPSERIAL_HAVE_DMA */ /* initial LED state */ drv_led_start(); @@ -263,7 +270,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { board_autoled_on(LED_RED); - return ret; } #ifdef HAVE_AUTOMOUNTER @@ -279,7 +285,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { board_autoled_on(LED_RED); - return ret; } #endif diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake deleted file mode 100644 index 8ad8201f1b83..000000000000 --- a/boards/nxp/fmuk66-v3/default.cmake +++ /dev/null @@ -1,130 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-v3 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/fxas21002c - imu/fxos8701cq - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board new file mode 100644 index 000000000000..f7bb26d45fcd --- /dev/null +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -0,0 +1,106 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BAROMETER_MPL3115A2=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_FXAS21002C=y +CONFIG_DRIVERS_IMU_FXOS8701CQ=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_LIGHTS_RGBLED=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y +CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y +CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y +CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_DRIVERS_UWB_UWB_SR150=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/nxp/fmuk66-v3/bootloader/nxp_fmuk66-v3_bootloader.bin b/boards/nxp/fmuk66-v3/extras/nxp_fmuk66-v3_bootloader.bin similarity index 100% rename from boards/nxp/fmuk66-v3/bootloader/nxp_fmuk66-v3_bootloader.bin rename to boards/nxp/fmuk66-v3/extras/nxp_fmuk66-v3_bootloader.bin diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index 750f12067358..df700b4dc350 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -3,6 +3,9 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 + rgbled_pwm start safety_button start diff --git a/boards/nxp/fmuk66-v3/init/rc.board_mavlink b/boards/nxp/fmuk66-v3/init/rc.board_mavlink deleted file mode 100644 index 8be295660bcd..000000000000 --- a/boards/nxp/fmuk66-v3/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# NXP fmuk66-v3 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig index 5bbf313271ed..b9d1df401c7a 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig @@ -5,12 +5,13 @@ # You can then do "make savedefconfig" to generate a new defconfig file that includes your # modifications. # +# CONFIG_KINETIS_EDMA_HOE is not set # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_SPI is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmuk66-v3/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="kinetis" @@ -33,7 +34,6 @@ CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=8000 CONFIG_CDCACM_VENDORID=0x1FC9 CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -42,6 +42,7 @@ CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_ETH0_PHY_TJA1100=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -66,7 +67,11 @@ CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y CONFIG_KINETIS_CRC=y -CONFIG_KINETIS_DMA=y +CONFIG_KINETIS_EDMA=y +CONFIG_KINETIS_EDMA_ELINK=y +CONFIG_KINETIS_EDMA_ERCA=y +CONFIG_KINETIS_EDMA_ERGA=y +CONFIG_KINETIS_EDMA_NTCD=16 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y CONFIG_KINETIS_ENET=y CONFIG_KINETIS_FLEXCAN0=y @@ -76,6 +81,7 @@ CONFIG_KINETIS_I2C0=y CONFIG_KINETIS_I2C1=y CONFIG_KINETIS_LPTMR0=y CONFIG_KINETIS_LPUART0=y +CONFIG_KINETIS_LPUART0_RXDMA=y CONFIG_KINETIS_MERGE_TTY=y CONFIG_KINETIS_PDB=y CONFIG_KINETIS_PIT=y @@ -104,7 +110,6 @@ CONFIG_KINETIS_UART_EXTEDED_BREAK=y CONFIG_KINETIS_UART_INVERT=y CONFIG_KINETIS_USBDCD=y CONFIG_KINETS_LPUART_LOWEST=y -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_STRERROR=y CONFIG_LPUART0_BAUD=57600 CONFIG_LPUART0_SERIAL_CONSOLE=y @@ -133,9 +138,7 @@ CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_WRITE_BUFFERS=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -143,11 +146,8 @@ CONFIG_NSH_DISABLE_DIRNAME=y CONFIG_NSH_DISABLE_EXPORT=y CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -171,7 +171,6 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 CONFIG_RAW_BINARY=y @@ -186,8 +185,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig index 5ea5fda9b9c7..906bd0ce855a 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig @@ -5,12 +5,13 @@ # You can then do "make savedefconfig" to generate a new defconfig file that includes your # modifications. # +# CONFIG_KINETIS_EDMA_HOE is not set # CONFIG_MMCSD_HAVE_WRITEPROTECT is not set # CONFIG_MMCSD_MMCSUPPORT is not set # CONFIG_MMCSD_SPI is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmuk66-v3/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="kinetis" @@ -25,6 +26,7 @@ CONFIG_BOARD_LOOPSPERMSEC=15175 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y +CONFIG_CAN_CONNS=1 CONFIG_CDCACM=y CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_PRODUCTID=0x001c @@ -62,11 +64,15 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_IOB_NBUFFERS=48 -CONFIG_IOB_NCHAINS=16 +CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y CONFIG_KINETIS_CRC=y -CONFIG_KINETIS_DMA=y +CONFIG_KINETIS_EDMA=y +CONFIG_KINETIS_EDMA_ELINK=y +CONFIG_KINETIS_EDMA_ERCA=y +CONFIG_KINETIS_EDMA_ERGA=y +CONFIG_KINETIS_EDMA_NTCD=16 CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y CONFIG_KINETIS_ENET=y CONFIG_KINETIS_FLEXCAN0=y @@ -76,6 +82,7 @@ CONFIG_KINETIS_I2C0=y CONFIG_KINETIS_I2C1=y CONFIG_KINETIS_LPTMR0=y CONFIG_KINETIS_LPUART0=y +CONFIG_KINETIS_LPUART0_RXDMA=y CONFIG_KINETIS_MERGE_TTY=y CONFIG_KINETIS_PDB=y CONFIG_KINETIS_PIT=y @@ -111,6 +118,7 @@ CONFIG_LPUART0_SERIAL_CONSOLE=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y CONFIG_MMCSD_SDIO=y CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y @@ -122,6 +130,7 @@ CONFIG_NETUTILS_TELNETD=y CONFIG_NET_BROADCAST=y CONFIG_NET_CAN=y CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_FILTER_MAX=0 CONFIG_NET_CAN_RAW_TX_DEADLINE=y CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_ICMP=y @@ -129,9 +138,7 @@ CONFIG_NET_ICMP_SOCKET=y CONFIG_NET_TCP=y CONFIG_NET_TIMESTAMP=y CONFIG_NET_UDP=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -140,11 +147,8 @@ CONFIG_NSH_DISABLE_DIRNAME=y CONFIG_NSH_DISABLE_EXPORT=y CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -168,7 +172,6 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x1fff0000 CONFIG_RAW_BINARY=y @@ -183,8 +186,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y diff --git a/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig new file mode 100644 index 000000000000..9a0ddfc5a1fb --- /dev/null +++ b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig @@ -0,0 +1,226 @@ +# +# 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_KINETIS_EDMA_HOE is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmuk66-v3/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="kinetis" +CONFIG_ARCH_CHIP_KINETIS=y +CONFIG_ARCH_CHIP_MK66FN2M0VMD18=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=15175 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_PRODUCTID=0x001c +CONFIG_CDCACM_PRODUCTSTR="PX4 FMUK66 v3.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_ETH0_PHY_TJA1100=y +CONFIG_EXAMPLES_CALIB_UDELAY=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_AUTOMOUNTER=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +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_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_KINETIS_ADC0=y +CONFIG_KINETIS_ADC1=y +CONFIG_KINETIS_CRC=y +CONFIG_KINETIS_EDMA=y +CONFIG_KINETIS_EDMA_ELINK=y +CONFIG_KINETIS_EDMA_ERCA=y +CONFIG_KINETIS_EDMA_ERGA=y +CONFIG_KINETIS_EDMA_NTCD=16 +CONFIG_KINETIS_EMAC_RMIICLK1588CLKIN=y +CONFIG_KINETIS_ENET=y +CONFIG_KINETIS_FLEXCAN0=y +CONFIG_KINETIS_FLEXCAN1=y +CONFIG_KINETIS_GPIOIRQ=y +CONFIG_KINETIS_I2C0=y +CONFIG_KINETIS_I2C1=y +CONFIG_KINETIS_LPTMR0=y +CONFIG_KINETIS_LPUART0=y +CONFIG_KINETIS_LPUART0_RXDMA=y +CONFIG_KINETIS_MERGE_TTY=y +CONFIG_KINETIS_PDB=y +CONFIG_KINETIS_PIT=y +CONFIG_KINETIS_PORTAINTS=y +CONFIG_KINETIS_PORTBINTS=y +CONFIG_KINETIS_PORTCINTS=y +CONFIG_KINETIS_PORTDINTS=y +CONFIG_KINETIS_PORTEINTS=y +CONFIG_KINETIS_RTC=y +CONFIG_KINETIS_SDHC=y +CONFIG_KINETIS_SERIALBRK_BSDCOMPAT=y +CONFIG_KINETIS_SPI0=y +CONFIG_KINETIS_SPI1=y +CONFIG_KINETIS_SPI2=y +CONFIG_KINETIS_UART0=y +CONFIG_KINETIS_UART0_RXDMA=y +CONFIG_KINETIS_UART1=y +CONFIG_KINETIS_UART1_RXDMA=y +CONFIG_KINETIS_UART2=y +CONFIG_KINETIS_UART2_RXDMA=y +CONFIG_KINETIS_UART4=y +CONFIG_KINETIS_UART4_RXDMA=y +CONFIG_KINETIS_UARTFIFOS=y +CONFIG_KINETIS_UART_BREAKS=y +CONFIG_KINETIS_UART_EXTEDED_BREAK=y +CONFIG_KINETIS_UART_INVERT=y +CONFIG_KINETIS_USBDCD=y +CONFIG_KINETS_LPUART_LOWEST=y +CONFIG_LIBC_STRERROR=y +CONFIG_LPUART0_BAUD=57600 +CONFIG_LPUART0_SERIAL_CONSOLE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NETDEV_PHY_IOCTL=y +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_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_SOLINGER=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_BASENAME=y +CONFIG_NSH_DISABLE_DIRNAME=y +CONFIG_NSH_DISABLE_EXPORT=y +CONFIG_NSH_DISABLE_HEXDUMP=y +CONFIG_NSH_DISABLE_LOSETUP=y +CONFIG_NSH_DISABLE_MKFIFO=y +CONFIG_NSH_DISABLE_MKRD=y +CONFIG_NSH_DISABLE_PUT=y +CONFIG_NSH_DISABLE_REBOOT=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_DISABLE_UNAME=y +CONFIG_NSH_DISABLE_WGET=y +CONFIG_NSH_DISABLE_XD=y +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_IOBUFFER_SIZE=256 +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_TELNETD_CLIENTSTACKSIZE=1280 +CONFIG_NSH_TELNETD_DAEMONSTACKSIZE=1024 +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x1fff0000 +CONFIG_RAW_BINARY=y +CONFIG_RTC=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=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_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CUTERM=y +CONFIG_SYSTEM_CUTERM_DEFAULT_BAUD=57600 +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TELNET_IOTHREAD_STACKSIZE=512 +CONFIG_TELNET_MAXLCLIENTS=1 +CONFIG_TELNET_RXBUFFER_SIZE=128 +CONFIG_TELNET_TXBUFFER_SIZE=128 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1100 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_IFLOWCONTROL=y +CONFIG_UART4_OFLOWCONTROL=y +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1100 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake deleted file mode 100644 index 3ca15b7956ec..000000000000 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ /dev/null @@ -1,130 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-v3 - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/fxas21002c - imu/fxos8701cq - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-v3/rtps.px4board b/boards/nxp/fmuk66-v3/rtps.px4board new file mode 100644 index 000000000000..81e62c7825b3 --- /dev/null +++ b/boards/nxp/fmuk66-v3/rtps.px4board @@ -0,0 +1 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake deleted file mode 100644 index f49551b4b1ae..000000000000 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ /dev/null @@ -1,130 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmuk66-v3 - LABEL socketcan - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS4 - TEL2:/dev/ttyS1 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - barometer/mpl3115a2 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - distance_sensor/srf05 # Specific driver - gps - #imu # all available imu drivers - imu/fxas21002c - imu/fxos8701cq - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan_v1 - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - dumpfile - esc_calib - #gpio - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/nxp/fmuk66-v3/socketcan.px4board b/boards/nxp/fmuk66-v3/socketcan.px4board new file mode 100644 index 000000000000..058d8efd6736 --- /dev/null +++ b/boards/nxp/fmuk66-v3/socketcan.px4board @@ -0,0 +1,9 @@ +CONFIG_DRIVERS_UAVCAN=n +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_ESC_SUBSCRIBER=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y +CONFIG_CYPHAL_READINESS_PUBLISHER=y +CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/boards/nxp/fmuk66-v3/src/board_config.h b/boards/nxp/fmuk66-v3/src/board_config.h index 51586205fe88..dcba86c32d41 100644 --- a/boards/nxp/fmuk66-v3/src/board_config.h +++ b/boards/nxp/fmuk66-v3/src/board_config.h @@ -253,12 +253,6 @@ __END_DECLS /* PTB6 ADC1_SE12 */ ADC1_GPIO(12), \ /* PTB7 ADC1_SE13 */ ADC1_GPIO(13) - - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - - /* User GPIOs * */ @@ -274,7 +268,6 @@ __END_DECLS // todo:Design this! #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 #define GPIO_ULTRASOUND_TRIGGER /* PTD0 */ (GPIO_LOWDRIVE | GPIO_OUTPUT_ZERO | PIN_PORTD | PIN0) #define GPIO_ULTRASOUND_ECHO /* PTA10 */ (GPIO_PULLUP | PIN_INT_BOTH | PIN_PORTA | PIN10) @@ -334,7 +327,6 @@ __END_DECLS #define BOARD_ADC_PERIPH_5V_OC (0) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_HAS_LED_PWM 1 @@ -343,7 +335,7 @@ __END_DECLS #define LED_TIM3_CH4OUT /* PTC8 RGB_B */ PIN_FTM3_CH4_1 /* This board provides a DMA pool and APIs */ -#define BOARD_DMA_ALLOC_POOL_SIZE 5120 +#define BOARD_DMA_ALLOC_POOL_SIZE 2048 /* This board provides the board_on_reset interface */ diff --git a/boards/nxp/fmuk66-v3/src/init.c b/boards/nxp/fmuk66-v3/src/init.c index e6fccc13ddcc..6d0c2f91fa9b 100644 --- a/boards/nxp/fmuk66-v3/src/init.c +++ b/boards/nxp/fmuk66-v3/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,7 @@ #include #include -#include -#include +#include #include "board_config.h" #include "arm_arch.h" @@ -188,6 +188,26 @@ kinetis_boardinitialize(void) VDD_3V3_SPEKTRUM_POWER_EN(true); } +/**************************************************************************** + * Name: kinetis_serial_dma_poll_all + * + * Description: + * Checks receive DMA buffers for received bytes that have not accumulated + * to the point where the DMA half/full interrupt has triggered. + * + * This function should be called from a timer or other periodic context. + * + ****************************************************************************/ + +void kinetis_lpserial_dma_poll_all(void) +{ +#if defined(LPSERIAL_HAVE_DMA) + kinetis_lpserial_dma_poll(); +#endif +#if defined(SERIAL_HAVE_DMA) + kinetis_serial_dma_poll(); +#endif +} /**************************************************************************** * Name: board_app_initialize @@ -234,24 +254,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA +#if defined(SERIAL_HAVE_DMA) || defined(LPSERIAL_HAVE_DMA) + // 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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)kinetis_serial_dma_poll, - NULL); -#endif + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)kinetis_lpserial_dma_poll_all, NULL); +#endif /* SERIAL_HAVE_DMA || LPSERIAL_HAVE_DMA */ /* initial LED state */ drv_led_start(); @@ -263,7 +270,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { board_autoled_on(LED_RED); - return ret; } #ifdef HAVE_AUTOMOUNTER @@ -279,7 +285,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { board_autoled_on(LED_RED); - return ret; } #endif diff --git a/boards/nxp/fmuk66-v3/test.px4board b/boards/nxp/fmuk66-v3/test.px4board new file mode 100644 index 000000000000..537c03c8664a --- /dev/null +++ b/boards/nxp/fmuk66-v3/test.px4board @@ -0,0 +1,9 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_BOARD_TESTING=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake deleted file mode 100644 index a2b8cdead9d6..000000000000 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ /dev/null @@ -1,130 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL fmurt1062-v1 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - LINKER_PREFIX ocram -# UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS1 - TEL1:/dev/ttyS2 - TEL2:/dev/ttyS3 - GPS2:/dev/ttyS4 - DRIVERS - #adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot # not ported - gps - #imu # all available imu drivers - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - #irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - #osd - #pca9685 - #pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - #roboclaw - #rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - #airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - #gyro_calibration - #gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - #vtol_att_control - SYSTEMCMDS - #bl_update # not ported - dmesg - dumpfile - esc_calib - #gpio - #hardfault_log # not ported - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board new file mode 100644 index 000000000000..8595e95f5e21 --- /dev/null +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -0,0 +1,74 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_LINKER_PREFIX="ocram" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_COMMON_UWB=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_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=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_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_GIMBAL=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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/fmurt1062-v1/init/rc.board_defaults b/boards/nxp/fmurt1062-v1/init/rc.board_defaults index 3b99b63eb3e3..9e9681767930 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_defaults +++ b/boards/nxp/fmurt1062-v1/init/rc.board_defaults @@ -3,5 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.1097 +param set-default BAT1_A_PER_V 15.391030303 + rgbled_pwm start safety_button start diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_mavlink b/boards/nxp/fmurt1062-v1/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/nxp/fmurt1062-v1/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig b/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig index 433c13fcad46..40f0d9dd0a64 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig +++ b/boards/nxp/fmurt1062-v1/nuttx-config/Kconfig @@ -20,7 +20,7 @@ config BOARD_HAS_PROBES default y ---help--- This board provides GPIO FMU-CH1-8, as PROBE_1-8 to provide timing signals - from selected drivers. + from selected drivers. config BOARD_USE_PROBES bool "Enable the use the board provided FMU-CH1-8 as PROBE_1-8" @@ -29,4 +29,13 @@ config BOARD_USE_PROBES ---help--- Select to use GPIO FMU-CH1-8, as PROBE_1-8 to provide timing signals - from selected drivers. + from selected drivers. + +config BOARD_FORCE_ALIGNMENT + bool "Forces all acesses to be Aligned" + default n + + ---help--- + Adds -mno-unaligned-access to build flags. to force alignment. + This can be needed if data is stored in a region of memory, that + is Strongly ordered and dcache is off. diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h b/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h index d0432dfe3c22..16ec5680874f 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h +++ b/boards/nxp/fmurt1062-v1/nuttx-config/include/board.h @@ -119,6 +119,8 @@ #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 diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig index 03ec39588728..fd6c7667e58b 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig @@ -10,7 +10,7 @@ # CONFIG_MMCSD_SPI is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/fmurt1062-v1/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="imxrt" @@ -20,7 +20,6 @@ CONFIG_ARCH_INTERRUPTSTACK=750 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DCACHE_WRITETHROUGH=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_ITCM=y @@ -29,12 +28,14 @@ CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CUSTOM_LEDS=y +CONFIG_BOARD_FORCE_ALIGNMENT=y CONFIG_BOARD_LOOPSPERMSEC=104926 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BOOT_RUNFROMISRAM=y CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_PRODUCTID=0x001d CONFIG_CDCACM_PRODUCTSTR="PX4 FMURT1062 v1.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -110,6 +111,7 @@ CONFIG_IMXRT_LPUART_INVERT=y CONFIG_IMXRT_LPUART_SINGLEWIRE=y CONFIG_IMXRT_RTC_MAGIC_REG=1 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 @@ -151,9 +153,7 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -164,8 +164,6 @@ CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y CONFIG_NSH_DISABLE_PSSTACKUSAGE=y @@ -189,10 +187,11 @@ CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y 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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -206,8 +205,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -224,6 +222,8 @@ CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DMA=y +CONFIG_USBDEV_DUALSPEED=y CONFIG_USBDEV_MAXPOWER=500 CONFIG_USEC_PER_TICK=1000 CONFIG_USERMAIN_STACKSIZE=2944 diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/scripts/ocram-script.ld b/boards/nxp/fmurt1062-v1/nuttx-config/scripts/ocram-script.ld index d2e6e75194ee..845fc6652629 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/scripts/ocram-script.ld +++ b/boards/nxp/fmurt1062-v1/nuttx-config/scripts/ocram-script.ld @@ -101,6 +101,7 @@ SECTIONS } > flash /* Catch all the section we want not in OCRAM so that the *(.text .text.*) in flash does not */ + /* Sift and sort with arm-none-eabi-nm -C -S -n build/nxp_fmurt1062-v1_default/nxp_fmurt1062-v1_default.elf > list.txt */ .flashxip : ALIGN(4) { @@ -108,8 +109,82 @@ SECTIONS /* Order matters */ *(.text.__start) *(.text.imxrt_ocram_initialize) - *(.slow_memory) + *(.text.romfs*) + *(.text.cromfs*) + *(.text.mpu*) + *(.text.arm_memfault*) + *(.text.arm_hardfault*) + *(.text.up_assert*) + *(.text.up_stackdump*) + *(.text.up_taskdump*) + *(.text.up_mdelay*) + *(.text.up_udelay*) + *(.text.board_on_reset*) + *(.text.board_spi_reset*) + *(.text.board_query_manifest*) + *(.text.board_reset*) + *(.text.board_get*) + *(.text.board_mcu*) + *(.text.imxrt_xbar_connect*) + *(.text.bson*) + *(.text.*print_load*) + *(.text.*px4_mft*) + *(.text.*px4_mtd*) + *(.text.syslog*) + *(.text.register_driver*) + *(.text.nx_start*) + *(.text.nx_bringup*) + *(.text.irq_unexpected_isr*) + *(.text.group*) + *(.text.*setenv*) + *(.text.*env*) + *(.text.cmd*) + *(.text.readline*) + *(.text.mkfatfs*) + *(.text.builtin*) + *(.text.basename*) + *(.text.dirname*) + *(.text.gmtime_r*) + *(.text.chdir*) + *(.text.devnull*) + *(.text.ramdisk*) + *(.text.files*) + *(.text.unregister_driver*) + *(.text.register_blockdriver*) + *(.text.bchdev_register*) + *(.text.part*) + *(.text.ftl*) + *(.text.*I2CBusIterator*) + *(.text.*SPIBusIterator*) + *(.text.*BusCLIArguments*) + *(.text.*WorkQueueManager*) + *(.text.*param_export*) + *(.text.*param_import*) + *(.text.*param_load*) + *(.text.*BusInstanceIterator*) + *(.text.*PRINT_MODULE_USAGE*) + *(.text.*px4_getopt*) + *(.text.*main*) + *(.text.*instantiate*) + *(.text.*ADC*) + *(.text.*MS5611*) + *(.text.*I2CSPIDriver*) + *(.text.*CameraCapture*) + *(.text.*i2cdetect*) + *(.text.*usage*) +/* *(.text.*Bosch*) 2% CPU .5% RAM */ + *(.text.*Tunes*) + *(.text.*printStatistics*) + *(.text.*init*) + *(.text.*test*) + *(.text.*task_spawn*) + *(.text.*custom_command*) + *(.text.*print_usage*) + *(.text.*print_status*) + *(.text.*status*) + *(.text.*CameraInterface*) + *(.text.*CameraTrigger*) *(.text.*ModuleBase*) *(.text.*print_message*) *(.text._ZN4Ekf2C2Eb) @@ -127,6 +202,8 @@ SECTIONS *(.text.*configure_streams_to_default*) *(.text.*_main) *(.text.*GPSDriverAshtech*) + *(.text.*GPSDriver*) + *(.text.*Mavlink*) *(.rodata .rodata.*) *(.fixup) *(.gnu.warning) diff --git a/boards/nxp/fmurt1062-v1/src/CMakeLists.txt b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt index d62c0da5eff5..c9976a96ed91 100644 --- a/boards/nxp/fmurt1062-v1/src/CMakeLists.txt +++ b/boards/nxp/fmurt1062-v1/src/CMakeLists.txt @@ -46,7 +46,6 @@ px4_add_library(drivers_board imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c ) -add_dependencies(drivers_board arch_board_hw_info) target_link_libraries(drivers_board PRIVATE diff --git a/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h index aa2dccac41b4..53accc0e6976 100644 --- a/boards/nxp/fmurt1062-v1/src/board_config.h +++ b/boards/nxp/fmurt1062-v1/src/board_config.h @@ -179,11 +179,6 @@ (1 << ADC_HW_REV_SENSE_CHANNEL) | \ (1 << ADC1_SPARE_1_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V */ - -#define BOARD_BATTERY1_V_DIV (10.1097f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) @@ -215,6 +210,7 @@ */ #define HEATER_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) #define GPIO_HEATER_OUTPUT /* GPIO_B1_09 QTIMER2_TIMER3 GPIO2_IO25 */ (GPIO_QTIMER2_TIMER3_1 | HEATER_IOMUX) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PWM Capture * @@ -232,6 +228,8 @@ #define GPIO_nARMED /* GPIO_SD_B1_01 GPIO3_IO1 */ (GPIO_PORT3 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | nARMED_OUTPUT_IOMUX) #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) + /* PWM */ @@ -240,7 +238,7 @@ // Input Capture not supported on MVP -#define DIRECT_INPUT_TIMER_CHANNELS 0 +#define BOARD_HAS_NO_CAPTURE //#define BOARD_HAS_UI_LED_PWM 1 Not ported yet (Still Kinetis driver) #define BOARD_HAS_LED_PWM 1 @@ -390,7 +388,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/nxp/fmurt1062-v1/src/init.c b/boards/nxp/fmurt1062-v1/src/init.c index 008d27e0d7f1..9f2bb13b987d 100644 --- a/boards/nxp/fmurt1062-v1/src/init.c +++ b/boards/nxp/fmurt1062-v1/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -165,7 +166,7 @@ __EXPORT void imxrt_ocram_initialize(void) uint32_t regval; /* Reallocate 128K of Flex RAM from ITCM to OCRAM - * Final Confiduration is + * Final Configuration is * 128 DTCM * * 128 FlexRAM OCRAM (202C:0000-202D:ffff) @@ -275,23 +276,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)imxrt_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)imxrt_serial_dma_poll, NULL); #endif /* initial LED state */ @@ -301,12 +289,12 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_off(LED_GREEN); led_off(LED_BLUE); + int ret = OK; #if defined(CONFIG_IMXRT_USDHC) - int ret = fmurt1062_usdhc_initialize(); + ret = fmurt1062_usdhc_initialize(); if (ret != OK) { led_on(LED_RED); - return ret; } #endif @@ -317,27 +305,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } /* Configure the HW based on the manifest */ px4_platform_configure(); - return OK; -} - -// USB Stubs -#include -void arm_usbinitialize(void) -{ -} - -int usbdev_register(struct usbdevclass_driver_s *driver) -{ - return -EINVAL; -} -int usbdev_unregister(struct usbdevclass_driver_s *driver) -{ - return -EINVAL; + return ret; } diff --git a/boards/nxp/fmurt1062-v1/src/manifest.c b/boards/nxp/fmurt1062-v1/src/manifest.c index 89c9327e21c9..8c1818294965 100644 --- a/boards/nxp/fmurt1062-v1/src/manifest.c +++ b/boards/nxp/fmurt1062-v1/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -48,6 +48,8 @@ #include #include +#include + #include "systemlib/px4_macros.h" #include "px4_log.h" @@ -121,7 +123,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - PX4_ERR("Board %4x is not supported!", ver_rev); + PX4_ERR("Board %4" PRIx32 " is not supported!", ver_rev); } } diff --git a/boards/nxp/ucans32k146/canbootloader.cmake b/boards/nxp/ucans32k146/canbootloader.cmake deleted file mode 100644 index 00b17e04afd8..000000000000 --- a/boards/nxp/ucans32k146/canbootloader.cmake +++ /dev/null @@ -1,13 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL ucans32k146 - LABEL canbootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - DRIVERS - bootloaders -) diff --git a/boards/nxp/ucans32k146/canbootloader.px4board b/boards/nxp/ucans32k146/canbootloader.px4board new file mode 100644 index 000000000000..46917280f6a4 --- /dev/null +++ b/boards/nxp/ucans32k146/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/nxp/ucans32k146/default.cmake b/boards/nxp/ucans32k146/default.cmake deleted file mode 100644 index c9c9006493f1..000000000000 --- a/boards/nxp/ucans32k146/default.cmake +++ /dev/null @@ -1,65 +0,0 @@ -include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -px4_add_board( - PLATFORM nuttx - VENDOR nxp - MODEL ucans32k146 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS1 - DRIVERS - #adc/board_adc - #barometer # all available barometer drivers - bootloaders - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - #dshot - gps - #imu # all available imu drivers - #lights - lights/rgbled_pwm - #magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - pwm_out - #safety_button - #tone_alarm - #uavcannode # TODO: CAN driver needed - #uavcan_v1 - uavcannode_gps_demo - MODULES - #ekf2 - #load_mon - #sensors - #temperature_compensation - SYSTEMCMDS - #bl_update - #dmesg - #dumpfile - #esc_calib - #hardfault_log - i2cdetect - led_control - mixer - mtd - mft - #motor_ramp - #motor_test - #nshterm - param - #perf - pwm - reboot - #reflect - #sd_bench - system_time - top - topic_listener - #tune_control - ver - work_queue -) diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board new file mode 100644 index 000000000000..50b749dafbd4 --- /dev/null +++ b/boards/nxp/ucans32k146/default.px4board @@ -0,0 +1,27 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_CLIENT=y +CONFIG_CYPHAL_APP_DESCRIPTOR=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index 5bccf6aa10ec..f9775ee814b7 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -4,3 +4,6 @@ #------------------------------------------------------------------------------ pwm_out mode_pwm1 start + +ifup can0 +cyphal start diff --git a/boards/nxp/ucans32k146/nuttx-config/canbootloader/defconfig b/boards/nxp/ucans32k146/nuttx-config/canbootloader/defconfig index 05f02bf57ee6..ce1602d8a02a 100644 --- a/boards/nxp/ucans32k146/nuttx-config/canbootloader/defconfig +++ b/boards/nxp/ucans32k146/nuttx-config/canbootloader/defconfig @@ -10,7 +10,7 @@ # CONFIG_SERIAL is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/ucans32k146/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="s32k1xx" @@ -43,16 +43,9 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=0 +CONFIG_FS_PROCFS_MAX_TASKS=0 CONFIG_NAME_MAX=0 CONFIG_NUNGET_CHARS=0 -CONFIG_NXFONTS_DISABLE_16BPP=y -CONFIG_NXFONTS_DISABLE_1BPP=y -CONFIG_NXFONTS_DISABLE_24BPP=y -CONFIG_NXFONTS_DISABLE_2BPP=y -CONFIG_NXFONTS_DISABLE_32BPP=y -CONFIG_NXFONTS_DISABLE_4BPP=y -CONFIG_NXFONTS_DISABLE_8BPP=y CONFIG_PREALLOC_TIMERS=0 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=126976 diff --git a/boards/nxp/ucans32k146/nuttx-config/include/board.h b/boards/nxp/ucans32k146/nuttx-config/include/board.h index 3c3d8a0234fb..5462c7ccb716 100644 --- a/boards/nxp/ucans32k146/nuttx-config/include/board.h +++ b/boards/nxp/ucans32k146/nuttx-config/include/board.h @@ -90,6 +90,12 @@ #define BOARD_LED_G_BIT (1 << BOARD_LED_G) #define BOARD_LED_B_BIT (1 << BOARD_LED_B) +/* Board revision detection pin + * 0 equals UCANS32K146-01 + * 1 equals UCANS32K146B + */ +#define BOARD_REVISION_DETECT_PIN (GPIO_INPUT | PIN_PORTA | PIN10 ) + /* If CONFIG_ARCH_LEDs is defined, then NuttX will control the LEDs on board * the UCANS32K146. The following definitions describe how NuttX * controls the LEDs: @@ -152,11 +158,13 @@ /* CAN selections ***********************************************************/ #define PIN_CAN0_TX PIN_CAN0_TX_4 /* PTE5 */ #define PIN_CAN0_RX PIN_CAN0_RX_4 /* PTE4 */ -#define PIN_CAN0_ENABLE (GPIO_OUTPUT | PIN_PORTE | PIN11 ) -#define CAN0_ENABLE_OUT 0 +#define PIN_CAN0_STB (GPIO_OUTPUT | PIN_PORTE | PIN11 ) +#define PIN_CAN0_ERRN (GPIO_INPUT | PIN_PORTA | PIN11 ) +#define PIN_CAN0_EN (GPIO_HIGHDRIVE | PIN_PORTA | PIN10 ) #define PIN_CAN1_TX PIN_CAN1_TX_1 /* PTA13 */ #define PIN_CAN1_RX PIN_CAN1_RX_1 /* PTA12 */ -#define PIN_CAN1_ENABLE (GPIO_OUTPUT | PIN_PORTE | PIN10 ) -#define CAN1_ENABLE_OUT 0 +#define PIN_CAN1_STB (GPIO_OUTPUT | PIN_PORTE | PIN10 ) +#define PIN_CAN1_ERRN (GPIO_PULLDOWN | PIN_PORTE | PIN6 ) +#define PIN_CAN1_EN (GPIO_OUTPUT | PIN_PORTE | PIN2 ) #endif /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */ diff --git a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig index 3bbed38e52aa..54fd7511e488 100644 --- a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig +++ b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig @@ -11,7 +11,7 @@ # CONFIG_NSH_CMDOPT_HEXDUMP is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/ucans32k146/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="s32k1xx" @@ -21,6 +21,7 @@ CONFIG_ARCH_CHIP_S32K1XX=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BCH=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_LOOPSPERMSEC=3997 CONFIG_BUILTIN=y @@ -33,6 +34,7 @@ CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_EXAMPLES_HELLO=y CONFIG_FS_CROMFS=y CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=16 CONFIG_FS_ROMFS=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y @@ -52,7 +54,6 @@ CONFIG_LPUART0_TXDMA=y CONFIG_LPUART1_RXBUFSIZE=128 CONFIG_LPUART1_SERIAL_CONSOLE=y CONFIG_LPUART1_TXBUFSIZE=128 -CONFIG_MAX_TASKS=16 CONFIG_MOTOROLA_SREC=y CONFIG_MTD=y CONFIG_MTD_PARTITION=y @@ -64,9 +65,7 @@ CONFIG_NET_CAN_NOTIFIER=y CONFIG_NET_CAN_RAW_TX_DEADLINE=y CONFIG_NET_CAN_SOCK_OPTS=y CONFIG_NET_TIMESTAMP=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_LINELEN=128 @@ -103,8 +102,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y -CONFIG_SPITOOL_DEFFREQ=400000 -CONFIG_SPITOOL_MAXBUS=0 +CONFIG_SPITOOL_MAXBUS=2 CONFIG_STACK_COLORATION=y CONFIG_START_DAY=18 CONFIG_START_MONTH=8 @@ -115,4 +113,5 @@ CONFIG_SYSTEM_I2CTOOL=y CONFIG_SYSTEM_NSH=y CONFIG_SYSTEM_SPITOOL=y CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_USERMAIN_STACKSIZE=2176 CONFIG_WATCHDOG=y diff --git a/boards/atlflight/eagle/src/init.c b/boards/nxp/ucans32k146/servo.px4board similarity index 100% rename from boards/atlflight/eagle/src/init.c rename to boards/nxp/ucans32k146/servo.px4board diff --git a/boards/nxp/ucans32k146/src/CMakeLists.txt b/boards/nxp/ucans32k146/src/CMakeLists.txt index 4a19e54f5116..e043e25fab86 100644 --- a/boards/nxp/ucans32k146/src/CMakeLists.txt +++ b/boards/nxp/ucans32k146/src/CMakeLists.txt @@ -45,7 +45,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") nuttx_arch nuttx_drivers canbootloader - arch_io_pins arch_led_pwm ) target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) @@ -58,11 +57,11 @@ else() bringup.c buttons.c clockconfig.c - # i2c.cpp # TODO + i2c.cpp init.c mtd.cpp periphclocks.c - spi.c + spi.cpp timer_config.cpp userleds.c ) @@ -73,6 +72,6 @@ else() nuttx_drivers # sdio drivers__led # drv_led_start px4_layer - arch_io_pins + px4_platform # I2CBusIterator ) endif() diff --git a/boards/nxp/ucans32k146/src/board_config.h b/boards/nxp/ucans32k146/src/board_config.h index 69c5ee3675d2..3e97c8d76b86 100644 --- a/boards/nxp/ucans32k146/src/board_config.h +++ b/boards/nxp/ucans32k146/src/board_config.h @@ -108,7 +108,6 @@ __BEGIN_DECLS #define DIRECT_PWM_OUTPUT_CHANNELS 1 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_HAS_LED_PWM 1 #define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 @@ -161,8 +160,9 @@ int s32k1xx_bringup(void); * ****************************************************************************/ -#ifdef CONFIG_S32K1XX_LPSPI +#ifdef CONFIG_S32K1XX_LPSPI0 void s32k1xx_spidev_initialize(void); +int s32k1xx_spi_bus_initialize(void); #endif /**************************************************************************************************** diff --git a/boards/nxp/ucans32k146/src/bringup.c b/boards/nxp/ucans32k146/src/bringup.c index ba381b25afe5..11a4f291d7b5 100644 --- a/boards/nxp/ucans32k146/src/bringup.c +++ b/boards/nxp/ucans32k146/src/bringup.c @@ -124,12 +124,13 @@ int s32k1xx_bringup(void) s32k1xx_eeeprom_register(0, 4096); #endif -#ifdef CONFIG_S32K1XX_LPSPI +#ifdef CONFIG_S32K1XX_LPSPI0 /* Configure SPI chip selects if 1) SPI is not disabled, and 2) the weak * function s32k1xx_spidev_initialize() has been brought into the link. */ s32k1xx_spidev_initialize(); + s32k1xx_spi_bus_initialize(); #endif #if defined(CONFIG_S32K1XX_LPI2C0) @@ -138,18 +139,35 @@ int s32k1xx_bringup(void) i2c = s32k1xx_i2cbus_initialize(0); if (i2c == NULL) { - serr("ERROR: Failed to get I2C%d interface\n", bus); + serr("ERROR: Failed to get I2C0 interface\n"); } else { ret = i2c_register(i2c, 0); if (ret < 0) { - serr("ERROR: Failed to register I2C%d driver: %d\n", bus, ret); + serr("ERROR: Failed to register I2C0 driver: %d\n", ret); s32k1xx_i2cbus_uninitialize(i2c); } } #endif #endif + +#ifdef CONFIG_S32K1XX_FLEXCAN + s32k1xx_pinconfig(BOARD_REVISION_DETECT_PIN); + + if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) { + /* STB high -> active CAN phy */ + s32k1xx_pinconfig(PIN_CAN0_STB | GPIO_OUTPUT_ONE); + s32k1xx_pinconfig(PIN_CAN1_STB | GPIO_OUTPUT_ONE); + + } else { + /* STB low -> active CAN phy */ + s32k1xx_pinconfig(PIN_CAN0_STB | GPIO_OUTPUT_ZERO); + s32k1xx_pinconfig(PIN_CAN1_STB | GPIO_OUTPUT_ZERO); + } + +#endif + return ret; } diff --git a/boards/nxp/ucans32k146/src/can_boot.c b/boards/nxp/ucans32k146/src/can_boot.c index b9213bb9012d..f6878bd71d22 100644 --- a/boards/nxp/ucans32k146/src/can_boot.c +++ b/boards/nxp/ucans32k146/src/can_boot.c @@ -71,7 +71,42 @@ __EXPORT void s32k1xx_board_initialize(void) // Can GPIO s32k1xx_pinconfig(PIN_CAN0_TX); s32k1xx_pinconfig(PIN_CAN0_RX); - s32k1xx_pinconfig(PIN_CAN0_ENABLE | GPIO_OUTPUT_ZERO); + + s32k1xx_pinconfig(BOARD_REVISION_DETECT_PIN); + + if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) { + /* Config Pins to do CAN tranceiver HW selftest */ + s32k1xx_pinconfig(PIN_CAN0_ERRN); + s32k1xx_pinconfig(PIN_CAN0_STB); + s32k1xx_pinconfig(PIN_CAN0_EN); + + /* EN high & STB high -> normal mode */ + s32k1xx_gpiowrite(PIN_CAN0_STB, 1); + s32k1xx_gpiowrite(PIN_CAN0_EN, 1); + up_udelay(3000); // Wait for startup to normal mode + + /* EN low & STB high -> listen only mode */ + s32k1xx_gpiowrite(PIN_CAN0_STB, 1); + s32k1xx_gpiowrite(PIN_CAN0_EN, 0); + up_udelay(100); // t moch ERRN_N + + /* Check for HW err and wait untill ERR has been cleared */ + while (!s32k1xx_gpioread(PIN_CAN0_ERRN)) { + board_indicate(hardware_failure); + s32k1xx_gpiowrite(PIN_CAN0_EN, 1); + up_udelay(50); + s32k1xx_gpiowrite(PIN_CAN0_EN, 0); + up_udelay(50); + } + + /* Enter normal-mode */ + s32k1xx_gpiowrite(PIN_CAN0_EN, 1); + + } else { + /* STB low -> active CAN phy */ + s32k1xx_pinconfig(PIN_CAN0_STB | GPIO_OUTPUT_ZERO); + } + //s32k1xx_gpiowrite #if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) s32k1xx_pinconfig(GPIO_GETNODEINFO_JUMPER); @@ -101,7 +136,14 @@ void board_deinitialize(void) } while ((regval & CAN_MCR_LPMACK) == 0); - s32k1xx_pinconfig(PIN_CAN0_ENABLE | GPIO_OUTPUT_ONE); + if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) { + /* STB high -> standby CAN phy */ + s32k1xx_pinconfig(PIN_CAN0_STB | GPIO_OUTPUT_ZERO); + + } else { + /* STB low -> standby CAN phy */ + s32k1xx_pinconfig(PIN_CAN0_STB | GPIO_OUTPUT_ONE); + } } /**************************************************************************** @@ -190,6 +232,7 @@ static const led_t i2l[] = { led(9, fw_update_timeout, 31, 0, 0, 2), led(a, fw_update_invalid_crc, 31, 0, 0, 4), led(b, jump_to_app, 0, 63, 0, 10), + led(c, hardware_failure, 31, 0, 0, 10), }; diff --git a/boards/nxp/ucans32k146/src/clockconfig.c b/boards/nxp/ucans32k146/src/clockconfig.c index 95e91ed28ea9..e197dfc55519 100644 --- a/boards/nxp/ucans32k146/src/clockconfig.c +++ b/boards/nxp/ucans32k146/src/clockconfig.c @@ -110,7 +110,7 @@ const struct clock_configuration_s g_initial_clkconfig = { { .mode = SCG_SPLL_MONITOR_DISABLE, /* SPLLCM */ .div1 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SPLLDIV1 */ - .div2 = SCG_ASYNC_CLOCK_DIV_BY_1, /* SPLLDIV2 */ + .div2 = SCG_ASYNC_CLOCK_DIV_BY_2, /* SPLLDIV2 */ .prediv = 1, /* PREDIV */ .mult = 40, /* MULT */ .src = 0, /* SOURCE */ diff --git a/boards/nxp/ucans32k146/src/i2c.cpp b/boards/nxp/ucans32k146/src/i2c.cpp index 75b583b8686f..25afc6218f0e 100644 --- a/boards/nxp/ucans32k146/src/i2c.cpp +++ b/boards/nxp/ucans32k146/src/i2c.cpp @@ -34,6 +34,5 @@ #include constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)), initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)), }; diff --git a/boards/nxp/ucans32k146/src/periphclocks.c b/boards/nxp/ucans32k146/src/periphclocks.c index b805597d826f..5dd570199b9e 100644 --- a/boards/nxp/ucans32k146/src/periphclocks.c +++ b/boards/nxp/ucans32k146/src/periphclocks.c @@ -101,7 +101,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { #else .clkgate = false, #endif - .clksrc = CLK_SRC_SIRC_DIV2, + .clksrc = CLK_SRC_SPLL_DIV2, }, { .clkname = LPSPI0_CLK, @@ -110,7 +110,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { #else .clkgate = false, #endif - .clksrc = CLK_SRC_SIRC_DIV2, + .clksrc = CLK_SRC_SPLL_DIV2, }, { .clkname = LPUART0_CLK, @@ -119,7 +119,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { #else .clkgate = false, #endif - .clksrc = CLK_SRC_SIRC_DIV2, + .clksrc = CLK_SRC_SPLL_DIV2, }, { .clkname = LPUART1_CLK, @@ -128,7 +128,7 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { #else .clkgate = false, #endif - .clksrc = CLK_SRC_SIRC_DIV2, + .clksrc = CLK_SRC_SPLL_DIV2, }, { .clkname = RTC0_CLK, diff --git a/boards/nxp/ucans32k146/src/spi.cpp b/boards/nxp/ucans32k146/src/spi.cpp new file mode 100644 index 000000000000..1e5708d259d4 --- /dev/null +++ b/boards/nxp/ucans32k146/src/spi.cpp @@ -0,0 +1,230 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018, 2021 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * Landon Haugh + * + * 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 + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "arm_arch.h" +#include "chip.h" +#include + +#include "s32k1xx_config.h" +#include "s32k1xx_lpspi.h" +#include "s32k1xx_pin.h" +#include "board_config.h" + +#if defined(CONFIG_S32K1XX_LPSPI0) +// Only LPSPI0 is defined in defconfig. cont. || defined(CONFIG_S32K1XX_LPSPI1) || defined(CONFIG_S32K1XX_LPSPI2) + +// UCANS32K146 has only one external SPI @ PTB5 +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBusExternal(SPI::Bus::SPI0, { + // Going to assume PTB5 means PortB, Pin5 + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin5}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +#define PX4_MK_GPIO(pin_ftmx, io) ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io)) + + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ + /* Goal not to back feed the chips on the bus via IO lines */ + + /* Next Change CS to inputs with pull downs */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + // s32k1xx_pinconfig is defined + // Only one argument, (uint32_t cfgset) + s32k1xx_pinconfig(PX4_MK_GPIO(px4_spi_buses[bus].devices[i].cs_gpio, GPIO_PULLDOWN)); + } + } + } + } + + /* Restore all the CS to ouputs inactive */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == PX4_BUS_NUMBER_TO_PX4(1)) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + s32k1xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } + } +} + +/************************************************************************************ + * Name: s32k1xx_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board. + * + ************************************************************************************/ + +void s32k1xx_spidev_initialize(void) +{ + board_spi_reset(10, 0xffff); + + 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) { + s32k1xx_pinconfig(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +/************************************************************************************ + * Name: s32k1xx_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP UCANS32K146 board. + * + ************************************************************************************/ + +static const px4_spi_bus_t *_spi_bus0; + +__EXPORT int s32k1xx_spi_bus_initialize(void) +{ + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 1: _spi_bus0 = &px4_spi_buses[i]; break; + } + } + + struct spi_dev_s *spi_ext = px4_spibus_initialize(1); + + if (!spi_ext) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); + return -ENODEV; + } + + /* Default external bus to 1MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_ext, 8 * 1000 * 1000); + SPI_SETBITS(spi_ext, 8); + SPI_SETMODE(spi_ext, 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_ext, px4_spi_buses[bus].devices[i].devid, false); + } + } + } + + return OK; + +} + +/************************************************************************************ + * Name: s32k1xx_spi[n]select, s32k1xx_spi[n]status, and s32k1xx_spi[n]cmddata + * + * Description: + * These external functions must be provided by board-specific logic. They are + * implementations of the select, status, and cmddata methods of the SPI interface + * defined by struct spi_ops_s (see include/nuttx/spi/spi.h). All other methods + * including s32k1xx_spibus_initialize()) are provided by common s32k1xx logic. + * To use this common SPI logic on your board: + * + * 1. Provide logic in s32k1xx_boardinitialize() to configure SPI chip select + * pins. + * 2. Provide s32k1xx_spi[n]select() and s32k1xx_spi[n]status() functions + * in your board-specific logic. These functions will perform chip selection + * and status operations using GPIOs in the way your board is configured. + * 2. If CONFIG_SPI_CMDDATA is defined in the NuttX configuration, provide + * s32k1xx_spi[n]cmddata() functions in your board-specific logic. These + * functions will perform cmd/data selection operations using GPIOs in the way + * your board is configured. + * 3. Add a call to s32k1xx_spibus_initialize() in your low level application + * initialization logic + * 4. The handle returned by s32k1xx_spibus_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 s32k1xx_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 + // s32k1xx_gpiowrite is defined (uint32_t pinset, bool value) + s32k1xx_gpiowrite(bus->devices[i].cs_gpio, !selected); + } + } +} + + +void s32k1xx_lpspi0select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert"); + s32k1xx_spixselect(_spi_bus0, dev, devid, selected); +} + +uint8_t s32k1xx_lpspi0status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} + +#endif /* CONFIG_S32K1XX_LPSPI0 */ diff --git a/boards/nxp/ucans32k146/uavcan_board_identity b/boards/nxp/ucans32k146/uavcan_board_identity index bf6272170ffb..dd7e3fcb776f 100644 --- a/boards/nxp/ucans32k146/uavcan_board_identity +++ b/boards/nxp/ucans32k146/uavcan_board_identity @@ -10,6 +10,8 @@ set(uavcanblid_hw_version_major 0) set(uavcanblid_hw_version_minor 34) set(uavcanblid_name "\"org.nxp.ucans32k146\"") +set(uavcanbl_padding 8) + add_definitions( -DHW_UAVCAN_NAME=${uavcanblid_name} -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake deleted file mode 100644 index fc7d0cf3f53c..000000000000 --- a/boards/omnibus/f4sd/default.cmake +++ /dev/null @@ -1,121 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR omnibus - MODEL f4sd - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - CONSTRAINED_FLASH - SERIAL_PORTS - TEL2:/dev/ttyS1 - URT6:/dev/ttyS2 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/bmp280 - #batt_smbus - #camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - dshot - gps - imu/invensense/icm20602 - imu/invensense/mpu6000 - #irlock - lights/rgbled - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - #optical_flow # all available optical flow drivers - osd - #pca9685 - #pwm_input - #pwm_out_sim - pwm_out - rc_input - #telemetry # all available telemetry drivers - telemetry/frsky_telemetry - #test_ppm - MODULES - #airspeed_selector - attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - #vmount - #vtol_att_control - SYSTEMCMDS - #bl_update - dmesg - dumpfile - esc_calib - #gpio - hardfault_log - i2cdetect - led_control - #mft - mixer - #motor_ramp - motor_test - #mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - #system_time - #tests # tests and test runner - top - #topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board new file mode 100644 index 000000000000..05506d3c2d06 --- /dev/null +++ b/boards/omnibus/f4sd/default.px4board @@ -0,0 +1,39 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=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_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PWM=y diff --git a/boards/omnibus/f4sd/extras/omnibus_f4sd_bootloader.bin b/boards/omnibus/f4sd/extras/omnibus_f4sd_bootloader.bin new file mode 100644 index 000000000000..be73a7f0edf0 Binary files /dev/null and b/boards/omnibus/f4sd/extras/omnibus_f4sd_bootloader.bin differ diff --git a/boards/omnibus/f4sd/icm20608g.px4board b/boards/omnibus/f4sd/icm20608g.px4board new file mode 100644 index 000000000000..b7b57eeb5e61 --- /dev/null +++ b/boards/omnibus/f4sd/icm20608g.px4board @@ -0,0 +1,3 @@ +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y diff --git a/boards/omnibus/f4sd/init/rc.board_defaults b/boards/omnibus/f4sd/init/rc.board_defaults index 97bbcd6ee691..5b492c435994 100644 --- a/boards/omnibus/f4sd/init/rc.board_defaults +++ b/boards/omnibus/f4sd/init/rc.board_defaults @@ -3,6 +3,9 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 11.12 +param set-default BAT1_A_PER_V 31 + # system_power unavailable param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/omnibus/f4sd/init/rc.board_mavlink b/boards/omnibus/f4sd/init/rc.board_mavlink deleted file mode 100644 index 93ae6863c926..000000000000 --- a/boards/omnibus/f4sd/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# OmnibusF4SD specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/omnibus/f4sd/init/rc.board_sensors b/boards/omnibus/f4sd/init/rc.board_sensors index 2fa62a88ff14..18ea496e69bb 100644 --- a/boards/omnibus/f4sd/init/rc.board_sensors +++ b/boards/omnibus/f4sd/init/rc.board_sensors @@ -8,7 +8,11 @@ board_adc start if ! mpu6000 -R 6 -s start then # some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602 - icm20602 -s -R 6 start + if ! icm20602 -s -R 6 start + then + # some clones of Omnibus F4 use the ICM-20608G (such as F4 PDB for Martian II FPV Drone) + icm20608g -s -R 6 start + fi fi bmp280 -s start diff --git a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig index cc162bbbdacf..76a57aada2ab 100644 --- a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig +++ b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig @@ -13,7 +13,7 @@ # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/omnibus/f4sd/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -30,6 +30,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0001 CONFIG_CDCACM_PRODUCTSTR="PX4 OmnibusF4SD" CONFIG_CDCACM_RXBUFSIZE=600 @@ -66,9 +67,7 @@ CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y CONFIG_MM_REGIONS=2 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_BASENAME=y @@ -78,11 +77,8 @@ CONFIG_NSH_DISABLE_HEXDUMP=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_LOSETUP=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y CONFIG_NSH_DISABLE_MKFIFO=y CONFIG_NSH_DISABLE_MKRD=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -119,8 +115,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y @@ -157,7 +152,7 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI2_DMA=y CONFIG_STM32_SPI3=y diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 72e7051e77eb..627a42e9c0cb 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -74,11 +74,6 @@ #define ADC_BATTERY_CURRENT_CHANNEL 11 #define ADC_RC_RSSI_CHANNEL 0 -/* Define Battery 1 Voltage Divider and A per V - */ -#define BOARD_BATTERY1_V_DIV (11.12f) -#define BOARD_BATTERY1_A_PER_V (31.f) - /* User GPIOs * * GPIO0-5 are the PWM servo outputs. @@ -119,10 +114,6 @@ * Alternatively CH3/CH4 could be assigned to UART6_TX/RX */ #define DIRECT_PWM_OUTPUT_CHANNELS 4 -#define DIRECT_INPUT_TIMER_CHANNELS 4 - -// Has pwm outputs -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* High-resolution timer */ #define HRT_TIMER 4 // T4C1 @@ -132,6 +123,7 @@ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) #define RC_SERIAL_PORT "/dev/ttyS0" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT /* * One RC_IN @@ -146,7 +138,6 @@ //#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX) //#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -156,7 +147,6 @@ #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {2, 3, 1, 0}; __BEGIN_DECLS diff --git a/boards/omnibus/f4sd/src/init.c b/boards/omnibus/f4sd/src/init.c index 9f85f9770721..746a544c82d6 100644 --- a/boards/omnibus/f4sd/src/init.c +++ b/boards/omnibus/f4sd/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -243,22 +244,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -277,7 +267,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); led_on(LED_BLUE); - return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ @@ -293,7 +282,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); led_on(LED_BLUE); - return -ENODEV; } /* Now bind the SPI interface to the MMCSD driver */ @@ -302,7 +290,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { led_on(LED_BLUE); syslog(LOG_ERR, "[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); - return -ENODEV; } up_udelay(20); @@ -314,7 +301,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi3) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 3\n"); led_on(LED_BLUE); - return -ENODEV; } /* Copied from fmu-v4 @@ -340,7 +326,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); led_on(LED_AMBER); - return -ENODEV; } #endif diff --git a/boards/omnibus/f4sd/src/spi.cpp b/boards/omnibus/f4sd/src/spi.cpp index 86794466f692..b3415ba8d3a4 100644 --- a/boards/omnibus/f4sd/src/spi.cpp +++ b/boards/omnibus/f4sd/src/spi.cpp @@ -39,6 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}), initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20608G, SPI::CS{GPIO::PortA, GPIO::Pin4}), }), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::PortB, GPIO::Pin12}) diff --git a/boards/omnibus/f4sd/src/timer_config.cpp b/boards/omnibus/f4sd/src/timer_config.cpp index c38c5b161ab3..fe452f3b3e61 100644 --- a/boards/omnibus/f4sd/src/timer_config.cpp +++ b/boards/omnibus/f4sd/src/timer_config.cpp @@ -34,8 +34,8 @@ #include constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { - initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}), initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake deleted file mode 100644 index 652547ca34c9..000000000000 --- a/boards/px4/fmu-v2/default.cmake +++ /dev/null @@ -1,144 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - #TESTING - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - #camera_capture - #camera_trigger - #differential_pressure # all available differential pressure drivers - differential_pressure/ms4525 - #distance_sensor # all available distance sensor drivers - #distance_sensor/ll40ls - #distance_sensor/lightware_laser_serial - #dshot - gps - #heater - #imu # all available imu drivers - #imu/analog_devices/adis16448 - imu/l3gd20 - imu/lsm303d - #imu/invensense/icm20608g - #imu/invensense/icm20948 - imu/invensense/mpu6000 - #imu/invensense/mpu9250 - #irlock - #lights # all available light drivers - lights/rgbled - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - #optical_flow # all available optical flow drivers - #osd - #pca9685 - #pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - #pwm_input - #pwm_out_sim - pwm_out - px4io - #roboclaw - #rpm - #telemetry # all available telemetry drivers - #test_ppm - tone_alarm - #uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - ekf2 - #esc_battery - #events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - #vmount - #vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - #dumpfile - #esc_calib - #gpio - hardfault_log - #i2cdetect - #led_control - mft - mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - #perf - pwm - reboot - #reflect - #sd_bench - #serial_test - #system_time - #tests # tests and test runner - top - #topic_listener - tune_control - #uorb - #usb_connected - #ver - #work_queue - EXAMPLES - #fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board new file mode 100644 index 000000000000..22d45c3a5e5a --- /dev/null +++ b/boards/px4/fmu-v2/default.px4board @@ -0,0 +1,47 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_L3GD20=y +CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_LIGHTS_RGBLED=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_TONE_ALARM=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_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_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_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_VER=y diff --git a/boards/px4/fmu-v2/bootloader/px4_fmu-v2_bootloader.bin b/boards/px4/fmu-v2/extras/px4_fmu-v2_bootloader.bin similarity index 100% rename from boards/px4/fmu-v2/bootloader/px4_fmu-v2_bootloader.bin rename to boards/px4/fmu-v2/extras/px4_fmu-v2_bootloader.bin diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake deleted file mode 100644 index e46b4510fc2e..000000000000 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ /dev/null @@ -1,91 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL fixedwing - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - #TESTING - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - distance_sensor/ll40ls - distance_sensor/lightware_laser_serial - gps - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - #imu/invensense/mpu9250 - lights/rgbled - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - pwm_out - px4io - #telemetry # all available telemetry drivers - telemetry/iridiumsbd - tone_alarm - #uavcan - MODULES - airspeed_selector - battery_status - camera_feedback - commander - dataman - ekf2 - #events - fw_att_control - fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - load_mon - logger - mavlink - navigator - rc_update - sensors - temperature_compensation - #vmount - SYSTEMCMDS - #bl_update - #dumpfile - #esc_calib - hardfault_log - #i2cdetect - #led_control - mft - mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - perf - pwm - reboot - #sd_bench - top - #topic_listener - tune_control - #uorb - #usb_connected - ver - #work_queue - ) diff --git a/boards/px4/fmu-v2/fixedwing.px4board b/boards/px4/fmu-v2/fixedwing.px4board new file mode 100644 index 000000000000..89fa7703c94a --- /dev/null +++ b/boards/px4/fmu-v2/fixedwing.px4board @@ -0,0 +1,12 @@ +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y diff --git a/boards/px4/fmu-v2/init/rc.board_defaults b/boards/px4/fmu-v2/init/rc.board_defaults new file mode 100644 index 000000000000..65348c4f846e --- /dev/null +++ b/boards/px4/fmu-v2/init/rc.board_defaults @@ -0,0 +1,8 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 + diff --git a/boards/px4/fmu-v2/init/rc.board_mavlink b/boards/px4/fmu-v2/init/rc.board_mavlink deleted file mode 100644 index cc337d34ae00..000000000000 --- a/boards/px4/fmu-v2/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv2 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 8aecc96c3467..f9a0c7217a71 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -1,6 +1,6 @@ #!/bin/sh # -# PX4 FMUv2 specific board sensors init +# board specific sensors init #------------------------------------------------------------------------------ rgbled start -I @@ -65,12 +65,6 @@ then # sensor heating is available, but we disable it for now param set-default SENS_EN_THERMAL 0 - # l3gd20 (external/isolated SPI4) - l3gd20 -s -b 4 -R 4 start - - # lsm303d (external/isolated SPI4) - lsm303d -s -b 4 -R 6 start - # ms5611 (external/isolated SPI4) ms5611 -s -b 4 start @@ -89,6 +83,13 @@ then mpu9250 -s -b 1 start fi + # l3gd20 (external/isolated SPI4) + # low quality sensor disabled by default to save memory + #l3gd20 -s -b 4 -R 4 start + + # lsm303d (external/isolated SPI4) + lsm303d -s -b 4 -R 6 start + else # $BOARD_FMUV3 = 0 -> FMUv2 diff --git a/boards/px4/fmu-v2/lto.px4board b/boards/px4/fmu-v2/lto.px4board new file mode 100644 index 000000000000..cca9cdd73beb --- /dev/null +++ b/boards/px4/fmu-v2/lto.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LTO=y diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake deleted file mode 100644 index f6b8011e2015..000000000000 --- a/boards/px4/fmu-v2/multicopter.cmake +++ /dev/null @@ -1,90 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL multicopter - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - barometer/ms5611 - #batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - gps - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - #imu/invensense/mpu9250 - irlock - lights/rgbled - magnetometer/hmc5883 - #optical_flow/px4flow - pwm_out - px4io - tone_alarm - MODULES - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - flight_mode_manager - gyro_calibration - #gyro_fft - #events - land_detector - landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - sensors - #sih - #temperature_compensation - #vmount - SYSTEMCMDS - #bl_update - #dumpfile - #esc_calib - hardfault_log - #i2cdetect - #led_control - mft - mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - perf - pwm - reboot - #sd_bench - top - #topic_listener - tune_control - #uorb - #usb_connected - ver - #work_queue - ) diff --git a/boards/px4/fmu-v2/multicopter.px4board b/boards/px4/fmu-v2/multicopter.px4board new file mode 100644 index 000000000000..b4e05c457957 --- /dev/null +++ b/boards/px4/fmu-v2/multicopter.px4board @@ -0,0 +1,2 @@ +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y diff --git a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig index d0487d27ba0f..9914efee90da 100644 --- a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig @@ -24,7 +24,7 @@ # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v2/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -34,6 +34,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -41,6 +42,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -82,8 +84,6 @@ CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_LIBC_STRERROR_SHORT=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +95,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 @@ -115,7 +110,6 @@ 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_PIPES=y CONFIG_PREALLOC_TIMERS=50 @@ -123,7 +117,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -140,8 +133,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -182,11 +174,11 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI4=y CONFIG_STM32_SPI4_DMA=y -CONFIG_STM32_SPI4_DMA_BUFFER=1024 +CONFIG_STM32_SPI4_DMA_BUFFER=512 CONFIG_STM32_SPI_DMA=y CONFIG_STM32_SPI_DMATHRESHOLD=32 CONFIG_STM32_TIM10=y diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake deleted file mode 100644 index 1955913e26c7..000000000000 --- a/boards/px4/fmu-v2/rover.cmake +++ /dev/null @@ -1,82 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL rover - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - CONSTRAINED_FLASH - - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - - DRIVERS - adc/board_adc - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - gps - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - #imu/invensense/mpu9250 - lights/rgbled - magnetometer/hmc5883 - optical_flow/px4flow - pwm_out - px4io - tone_alarm - - MODULES - camera_feedback - commander - dataman - ekf2 - events - rover_pos_control - land_detector - load_mon - logger - mavlink - navigator - battery_status - rc_update - sensors - temperature_compensation - #vmount - - SYSTEMCMDS - bl_update - #dumpfile - #esc_calib - hardfault_log - i2cdetect - #led_control - mft - mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - perf - pwm - reboot - #sd_bench - top - #topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v2/rover.px4board b/boards/px4/fmu-v2/rover.px4board new file mode 100644 index 000000000000..6f6ae3b190c0 --- /dev/null +++ b/boards/px4/fmu-v2/rover.px4board @@ -0,0 +1,16 @@ +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v2/src/board_config.h b/boards/px4/fmu-v2/src/board_config.h index 0ef74dad9b23..643e175c7ae0 100644 --- a/boards/px4/fmu-v2/src/board_config.h +++ b/boards/px4/fmu-v2/src/board_config.h @@ -108,12 +108,6 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -131,7 +125,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* USB OTG FS * @@ -159,14 +152,12 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v2/src/init.c b/boards/px4/fmu-v2/src/init.c index d62c7b72f063..61cc923fd605 100644 --- a/boards/px4/fmu-v2/src/init.c +++ b/boards/px4/fmu-v2/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -160,7 +161,7 @@ __EXPORT void board_on_reset(int status) up_mdelay(400); /* on reboot (status >= 0) reset sensors and peripherals */ - board_spi_reset(10, 0xffff); + board_control_spi_sensors_power(false, 0xffff); } } @@ -431,22 +432,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -463,7 +453,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ @@ -479,7 +468,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) @@ -495,7 +483,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi4) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 4); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI4 to 1MHz and de-assert the known chip selects. */ @@ -511,7 +498,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio) { led_on(LED_AMBER); syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ @@ -520,7 +506,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_AMBER); syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ diff --git a/boards/px4/fmu-v2/src/manifest.c b/boards/px4/fmu-v2/src/manifest.c index e93a7bbeba46..a814ab396122 100644 --- a/boards/px4/fmu-v2/src/manifest.c +++ b/boards/px4/fmu-v2/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -50,7 +50,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -136,7 +138,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v2/src/spi.cpp b/boards/px4/fmu-v2/src/spi.cpp index ce80e1cee487..e28958d3baba 100644 --- a/boards/px4/fmu-v2/src/spi.cpp +++ b/boards/px4/fmu-v2/src/spi.cpp @@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) }), initSPIBusExternal(SPI::Bus::SPI4, { - initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), }), }), diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake deleted file mode 100644 index 579b867d94ca..000000000000 --- a/boards/px4/fmu-v2/test.cmake +++ /dev/null @@ -1,132 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL test - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_test - IO px4_io-v2_default - TESTING - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - #camera_capture - #camera_trigger - #differential_pressure # all available differential pressure drivers - differential_pressure/ms4525 - #distance_sensor # all available distance sensor drivers - distance_sensor/ll40ls - distance_sensor/lightware_laser_serial - #dshot - gps - #heater - #imu # all available imu drivers - #imu/analog_devices/adis16448 - #imu/adis16477 - #imu/adis16497 - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - #imu/invensense/mpu9250 - #iridiumsbd - #irlock - lights/rgbled - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - #optical_flow # all available optical flow drivers - optical_flow/px4flow - #osd - #pca9685 - #protocol_splitter - #pwm_input - #pwm_out_sim - pwm_out - px4io - #roboclaw - #telemetry # all available telemetry drivers - #test_ppm - tone_alarm - #uavcan - MODULES - #airspeed_selector - #attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - #ekf2 - #events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #vmount - #vtol_att_control - SYSTEMCMDS - #bl_update - #dmesg - #dumpfile - #esc_calib - hardfault_log - #i2cdetect - #led_control - mft - mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - perf - pwm - reboot - #reflect - #sd_bench - #shutdown - tests # tests and test runner - top - #topic_listener - tune_control - #uorb - #usb_connected - ver - #work_queue - EXAMPLES - #fake_gps - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake deleted file mode 100644 index 7a6c719a8876..000000000000 --- a/boards/px4/fmu-v3/default.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v3 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/l3gd20 - imu/lsm303d - imu/invensense/icm20608g - imu/invensense/icm20948 - imu/invensense/mpu6000 - imu/invensense/mpu9250 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board new file mode 100644 index 000000000000..7d2c17bea312 --- /dev/null +++ b/boards/px4/fmu-v3/default.px4board @@ -0,0 +1,110 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IMU_L3GD20=y +CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v3/bootloader/px4_fmu-v3_bootloader.bin b/boards/px4/fmu-v3/extras/px4_fmu-v3_bootloader.bin similarity index 100% rename from boards/px4/fmu-v3/bootloader/px4_fmu-v3_bootloader.bin rename to boards/px4/fmu-v3/extras/px4_fmu-v3_bootloader.bin diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/init/rc.board_defaults b/boards/px4/fmu-v3/init/rc.board_defaults new file mode 100644 index 000000000000..65348c4f846e --- /dev/null +++ b/boards/px4/fmu-v3/init/rc.board_defaults @@ -0,0 +1,8 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.177939394 +param set-default BAT1_A_PER_V 15.391030303 + diff --git a/boards/px4/fmu-v3/init/rc.board_mavlink b/boards/px4/fmu-v3/init/rc.board_mavlink deleted file mode 100644 index c915fefe8c72..000000000000 --- a/boards/px4/fmu-v3/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv3 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 33e5698d7630..f9a0c7217a71 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -1,6 +1,6 @@ #!/bin/sh # -# PX4 FMUv3 specific board sensors init +# board specific sensors init #------------------------------------------------------------------------------ rgbled start -I @@ -65,12 +65,6 @@ then # sensor heating is available, but we disable it for now param set-default SENS_EN_THERMAL 0 - # l3gd20 (external/isolated SPI4) - l3gd20 -s -b 4 -R 4 start - - # lsm303d (external/isolated SPI4) - lsm303d -s -b 4 -R 6 start - # ms5611 (external/isolated SPI4) ms5611 -s -b 4 start @@ -89,6 +83,13 @@ then mpu9250 -s -b 1 start fi + # l3gd20 (external/isolated SPI4) + # low quality sensor disabled by default to save memory + #l3gd20 -s -b 4 -R 4 start + + # lsm303d (external/isolated SPI4) + lsm303d -s -b 4 -R 6 start + else # $BOARD_FMUV3 = 0 -> FMUv2 diff --git a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig index 73c428d934d8..63f116299895 100644 --- a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v3/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -34,6 +35,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -41,13 +43,13 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=2000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -56,6 +58,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -76,7 +79,6 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y @@ -90,18 +92,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,7 +114,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -134,8 +130,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -176,11 +171,11 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI4=y CONFIG_STM32_SPI4_DMA=y -CONFIG_STM32_SPI4_DMA_BUFFER=1024 +CONFIG_STM32_SPI4_DMA_BUFFER=512 CONFIG_STM32_SPI_DMA=y CONFIG_STM32_SPI_DMATHRESHOLD=32 CONFIG_STM32_TIM10=y diff --git a/boards/px4/fmu-v3/nuttx-config/test/defconfig b/boards/px4/fmu-v3/nuttx-config/test/defconfig new file mode 100644 index 000000000000..9f2cc8b1fb2f --- /dev/null +++ b/boards/px4/fmu-v3/nuttx-config/test/defconfig @@ -0,0 +1,236 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v3/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_STM32F427V=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=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_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_LIBC_LONG_LONG=y +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=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +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_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_I=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=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=512 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI4=y +CONFIG_STM32_SPI4_DMA=y +CONFIG_STM32_SPI4_DMA_BUFFER=512 +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=32 +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=128 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=600 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index c03c486de6a0..3c12d70e138d 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -108,12 +108,6 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - /* Power supply control and monitoring GPIOs */ #define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) @@ -131,7 +125,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* USB OTG FS * @@ -159,14 +152,12 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; /* Internal IMU Heater * diff --git a/boards/px4/fmu-v3/src/init.c b/boards/px4/fmu-v3/src/init.c index d62c7b72f063..297318496116 100644 --- a/boards/px4/fmu-v3/src/init.c +++ b/boards/px4/fmu-v3/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -160,7 +161,7 @@ __EXPORT void board_on_reset(int status) up_mdelay(400); /* on reboot (status >= 0) reset sensors and peripherals */ - board_spi_reset(10, 0xffff); + board_control_spi_sensors_power(false, 0xffff); } } @@ -431,22 +432,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -463,7 +453,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ @@ -479,7 +468,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) @@ -495,7 +483,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi4) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 4); led_on(LED_AMBER); - return -ENODEV; } /* Default SPI4 to 1MHz and de-assert the known chip selects. */ @@ -511,7 +498,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio) { led_on(LED_AMBER); syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; } /* Now bind the SDIO interface to the MMC/SD driver */ @@ -520,7 +506,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_AMBER); syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ @@ -529,7 +514,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif /* Configure the HW based on the manifest */ - px4_platform_configure(); return OK; diff --git a/boards/px4/fmu-v3/src/manifest.c b/boards/px4/fmu-v3/src/manifest.c index e93a7bbeba46..a814ab396122 100644 --- a/boards/px4/fmu-v3/src/manifest.c +++ b/boards/px4/fmu-v3/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -50,7 +50,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -136,7 +138,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v3/src/spi.cpp b/boards/px4/fmu-v3/src/spi.cpp index ce80e1cee487..e28958d3baba 100644 --- a/boards/px4/fmu-v3/src/spi.cpp +++ b/boards/px4/fmu-v3/src/spi.cpp @@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}) }), initSPIBusExternal(SPI::Bus::SPI4, { - initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}), + initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}), }), }), diff --git a/boards/px4/fmu-v3/test.px4board b/boards/px4/fmu-v3/test.px4board new file mode 100644 index 000000000000..27cedc328f4f --- /dev/null +++ b/boards/px4/fmu-v3/test.px4board @@ -0,0 +1,11 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake deleted file mode 100644 index fb922141b674..000000000000 --- a/boards/px4/fmu-v4/cannode.cmake +++ /dev/null @@ -1,91 +0,0 @@ - - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.px4.fmu-v4_cannode\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL cannode - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT cannode - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - bootloaders - #differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot - gps - #imu # all available imu drivers - #imu/analog_devices/adis16448 - #imu/adis16477 - #imu/adis16497 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - #lights/rgbled - #lights/rgbled_ncp5623c - #magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - #pwm_out - #safety_button - #tone_alarm - uavcannode - MODULES - #ekf2 - #load_mon - sensors - #temperature_compensation - SYSTEMCMDS - #bl_update - #dmesg - #dumpfile - #esc_calib - #hardfault_log - i2cdetect - mft - #led_control - #mixer - #motor_ramp - #motor_test - mtd - #nshterm - param - perf - #pwm - reboot - #reflect - #sd_bench - system_time - #shutdown - top - #topic_listener - #tune_control - ver - work_queue -) diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake deleted file mode 100644 index 09fe3655bc29..000000000000 --- a/boards/px4/fmu-v4/default.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - WIFI:/dev/ttyS0 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board new file mode 100644 index 000000000000..39b899314b34 --- /dev/null +++ b/boards/px4/fmu-v4/default.px4board @@ -0,0 +1,111 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_WIFI="/dev/ttyS0" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v4/bootloader/px4_fmu-v4_bootloader.bin b/boards/px4/fmu-v4/extras/px4_fmu-v4_bootloader.bin similarity index 100% rename from boards/px4/fmu-v4/bootloader/px4_fmu-v4_bootloader.bin rename to boards/px4/fmu-v4/extras/px4_fmu-v4_bootloader.bin diff --git a/boards/px4/fmu-v4/init/rc.board_defaults b/boards/px4/fmu-v4/init/rc.board_defaults index d08737ebc2db..275644b21f89 100644 --- a/boards/px4/fmu-v4/init/rc.board_defaults +++ b/boards/px4/fmu-v4/init/rc.board_defaults @@ -3,26 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ - -# start MAVLink on Wifi (ESP8266 port). Except for the TealOne airframe. -if ! param compare SYS_AUTOSTART 4250 -then - param set-default MAV_2_CONFIG 301 - param set-default MAV_2_RATE 20000 - param set-default SER_WIFI_BAUD 921600 -fi - -if param compare SER_WIFI_BAUD 1 -then - # Transitional support: The Wifi port has not been configured by the user, - # configure it for MAVLink via the ESP8266 Wifi module. Except for the TealOne airframe. - if ! param compare SYS_AUTOSTART 4250 - then - param set-default MAV_2_CONFIG 301 - param set-default MAV_2_RATE 20000 - param set-default SER_WIFI_BAUD 921600 - fi -fi - +param set-default BAT1_V_DIV 13.653333333 +param set-default BAT1_A_PER_V 36.367515152 safety_button start diff --git a/boards/px4/fmu-v4/init/rc.board_mavlink b/boards/px4/fmu-v4/init/rc.board_mavlink deleted file mode 100644 index 90328850a83b..000000000000 --- a/boards/px4/fmu-v4/init/rc.board_mavlink +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv4 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 - diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index b6f2662f1734..449d0389a105 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -5,19 +5,11 @@ board_adc start -# We know there are sketchy boards out there -# as chinese companies produce Pixracers without -# fully understanding the critical parts of the -# schematic and BOM, leading to sensor brownouts -# on boot. Original Pixracers following the -# open hardware design do not require this. -pwm_out sensor_reset 50 - # Internal SPI ms5611 -s start # hmc5883 internal SPI bus is rotated 90 deg yaw -if ! hmc5883 -T -s -R 2 start +if ! hmc5883 -T -s -R 2 -q start then # lis3mdl internal SPI bus is rotated 90 deg yaw lis3mdl -s start @@ -40,5 +32,5 @@ then mpu6500 -s -R 0 start else # mpu9250 internal SPI bus mpu9250 - mpu9250 -s -R 8 start + mpu9250 -s -R 8 -M start fi diff --git a/boards/atlflight/excelsior/src/init.c b/boards/px4/fmu-v4/nuttx-config/drivers/Kconfig similarity index 100% rename from boards/atlflight/excelsior/src/init.c rename to boards/px4/fmu-v4/nuttx-config/drivers/Kconfig diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index a060e8ab22de..afa537f7eba7 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v4/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -34,6 +35,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -41,13 +43,13 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=2000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -56,6 +58,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -76,7 +79,6 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y @@ -90,18 +92,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,7 +114,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -134,8 +130,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -175,7 +170,7 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI4=y CONFIG_STM32_SPI_DMA=y diff --git a/boards/px4/fmu-v4/nuttx-config/scripts/script.ld b/boards/px4/fmu-v4/nuttx-config/scripts/script.ld index 6d92733df886..ff95ca6ba14e 100644 --- a/boards/px4/fmu-v4/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v4/nuttx-config/scripts/script.ld @@ -79,6 +79,7 @@ SECTIONS _bootdelay_signature = ABSOLUTE(.); FILL(0xffecc2925d7d05c5) . += 8; + KEEP(*(.app_descriptor)) *(.text .text.*) *(.fixup) *(.gnu.warning) diff --git a/boards/px4/fmu-v4/nuttx-config/test/defconfig b/boards/px4/fmu-v4/nuttx-config/test/defconfig new file mode 100644 index 000000000000..fd63253ba166 --- /dev/null +++ b/boards/px4/fmu-v4/nuttx-config/test/defconfig @@ -0,0 +1,234 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v4/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_STM32F427V=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0012 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=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_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_LIBC_LONG_LONG=y +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=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +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_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_I=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=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=512 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI4=y +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=8 +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM8=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=1200 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=900 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4/src/CMakeLists.txt b/boards/px4/fmu-v4/src/CMakeLists.txt index 8730ec995917..e28e360c2329 100644 --- a/boards/px4/fmu-v4/src/CMakeLists.txt +++ b/boards/px4/fmu-v4/src/CMakeLists.txt @@ -48,5 +48,4 @@ target_link_libraries(drivers_board nuttx_arch # sdio nuttx_drivers # sdio px4_layer - arch_io_pins ) diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index a82209719997..ef6571d84d78 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -83,11 +83,6 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_RC_RSSI_CHANNEL 11 -/* Define Battery 1 Voltage Divider and A per V. */ -#define BOARD_BATTERY1_V_DIV (13.653333333f) -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - - /* Power supply control and monitoring GPIOs. */ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) @@ -104,7 +99,6 @@ * Six PWM outputs are configured. */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /** * USB OTG FS: @@ -147,6 +141,7 @@ /* Heater pins */ #define GPIO_HEATER_INPUT (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6) #define GPIO_HEATER_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* Power switch controls */ @@ -178,14 +173,12 @@ #define BOARD_ADC_PERIPH_5V_OC (0) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v4/src/init.c b/boards/px4/fmu-v4/src/init.c index 688be8a56d34..adf97b680012 100644 --- a/boards/px4/fmu-v4/src/init.c +++ b/boards/px4/fmu-v4/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -266,22 +267,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - // Set up the serial DMA polling. +#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; - struct timespec ts; - - /** - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif // Initial LED state. drv_led_start(); @@ -302,7 +292,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); led_on(LED_RED); - return -ENODEV; } @@ -318,7 +307,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); led_on(LED_RED); - return -ENODEV; } /** @@ -360,9 +348,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio) { led_on(LED_RED); - syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); } // Now bind the SDIO interface to the MMC/SD driver. @@ -371,7 +357,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } // Then let's guess and say that there is a card in the slot. There is no card detect GPIO. diff --git a/boards/px4/fmu-v4/test.px4board b/boards/px4/fmu-v4/test.px4board new file mode 100644 index 000000000000..27cedc328f4f --- /dev/null +++ b/boards/px4/fmu-v4/test.px4board @@ -0,0 +1,11 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/px4/fmu-v4/uavcan_board_identity b/boards/px4/fmu-v4/uavcan_board_identity new file mode 100644 index 000000000000..0905752e4958 --- /dev/null +++ b/boards/px4/fmu-v4/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 1) +set(uavcanblid_hw_version_minor 0) +set(uavcanblid_name "\"org.px4.fmu-v4_cannode\"") + +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/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake deleted file mode 100644 index 8921963c77ed..000000000000 --- a/boards/px4/fmu-v4pro/default.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4pro - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL3:/dev/ttyS0 - TEL4:/dev/ttyS6 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - rpm - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board new file mode 100644 index 000000000000..bc37bab8b703 --- /dev/null +++ b/boards/px4/fmu-v4pro/default.px4board @@ -0,0 +1,107 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v4pro/bootloader/px4_fmu-v4pro_bootloader.bin b/boards/px4/fmu-v4pro/extras/px4_fmu-v4pro_bootloader.bin similarity index 100% rename from boards/px4/fmu-v4pro/bootloader/px4_fmu-v4pro_bootloader.bin rename to boards/px4/fmu-v4pro/extras/px4_fmu-v4pro_bootloader.bin diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v4pro/init/rc.board_defaults b/boards/px4/fmu-v4pro/init/rc.board_defaults index f881b43a5cac..8dd9df28d5a2 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_defaults +++ b/boards/px4/fmu-v4pro/init/rc.board_defaults @@ -3,6 +3,12 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 6.490196078 +param set-default BAT2_V_DIV 6.490196078 + +param set-default BAT1_A_PER_V 26.4 +param set-default BAT2_A_PER_V 26.4 + # Multi-EKF param set-default EKF2_MULTI_IMU 2 param set-default SENS_IMU_MODE 0 diff --git a/boards/px4/fmu-v4pro/init/rc.board_mavlink b/boards/px4/fmu-v4pro/init/rc.board_mavlink deleted file mode 100644 index 9302d547fa35..000000000000 --- a/boards/px4/fmu-v4pro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv4pro specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index c687c96a4ce5..7906afc7e2f0 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ rgbled start -I -rgbled_ncp5623c start -I +rgbled_ncp5623c start -I -q board_adc start diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig index 792aefb75cad..9ac90eefa8f0 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v4pro/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -34,6 +35,7 @@ CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=16717 @@ -41,13 +43,13 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0013 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x PRO" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=2000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -56,6 +58,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -76,7 +79,6 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y @@ -90,18 +92,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,10 +114,11 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=393216 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -134,8 +132,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -179,7 +176,7 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI5=y CONFIG_STM32_SPI6=y diff --git a/boards/px4/fmu-v4pro/nuttx-config/scripts/script.ld b/boards/px4/fmu-v4pro/nuttx-config/scripts/script.ld index 5e657267ef75..d43f2e7f3bcd 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v4pro/nuttx-config/scripts/script.ld @@ -66,6 +66,7 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ */ EXTERN(abort) EXTERN(_bootdelay_signature) +EXTERN(board_get_manifest) SECTIONS { diff --git a/boards/px4/fmu-v4pro/nuttx-config/test/defconfig b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig new file mode 100644 index 000000000000..ade487fadf6b --- /dev/null +++ b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig @@ -0,0 +1,242 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v4pro/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_STM32F469I=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0013 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x PRO" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=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_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_LIBC_LONG_LONG=y +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=2 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=393216 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +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_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_BBSRAM=y +CONFIG_STM32_BBSRAM_FILES=5 +CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CCMDATARAM=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_I=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_I2C1=y +CONFIG_STM32_I2C2=y +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=10 +CONFIG_STM32_I2SPLL=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_OTGFS=y +CONFIG_STM32_PWR=y +CONFIG_STM32_RTC=y +CONFIG_STM32_RTC_HSECLOCK=y +CONFIG_STM32_RTC_MAGIC=0xfacefeee +CONFIG_STM32_RTC_MAGIC_REG=1 +CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef +CONFIG_STM32_SAIPLL=y +CONFIG_STM32_SAVE_CRASHDUMP=y +CONFIG_STM32_SDIO=y +CONFIG_STM32_SDIO_CARD=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=512 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI5=y +CONFIG_STM32_SPI6=y +CONFIG_STM32_SPI_DMA=y +CONFIG_STM32_SPI_DMATHRESHOLD=8 +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM8=y +CONFIG_STM32_TIM9=y +CONFIG_STM32_UART4=y +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +CONFIG_STM32_USART6=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_USART_SINGLEWIRE=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=300 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=300 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=300 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=300 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=300 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=300 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_RXDMA=y +CONFIG_USART1_TXBUFSIZE=2500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=300 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=600 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=300 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4pro/src/CMakeLists.txt b/boards/px4/fmu-v4pro/src/CMakeLists.txt index a7db4590f0da..bb72c52269a3 100644 --- a/boards/px4/fmu-v4pro/src/CMakeLists.txt +++ b/boards/px4/fmu-v4pro/src/CMakeLists.txt @@ -36,6 +36,7 @@ add_library(drivers_board i2c.cpp init.c led.c + mtd.cpp spi.cpp timer_config.cpp usb.c diff --git a/boards/px4/fmu-v4pro/src/board_config.h b/boards/px4/fmu-v4pro/src/board_config.h index c759fb714ff7..e45c81f11656 100644 --- a/boards/px4/fmu-v4pro/src/board_config.h +++ b/boards/px4/fmu-v4pro/src/board_config.h @@ -112,18 +112,6 @@ (1 << ADC_5V_RAIL_SENSE) | \ (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | (1 << ADC_BATTERY2_CURRENT_CHANNEL) -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (6.490196078f) -#define BOARD_BATTERY1_A_PER_V (26.4f) - -/* Define Battery 2 Voltage Divider and A per V - */ - -#define BOARD_BATTERY2_V_DIV (6.490196078f) -#define BOARD_BATTERY2_A_PER_V (26.4f) - /* Define LTC4417 UV set by resistors on the board that are different than FMUv2 3.7V */ #define BOARD_VALID_UV (4.01f) @@ -148,7 +136,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /* USB OTG FS * @@ -196,7 +183,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/px4/fmu-v4pro/src/init.c b/boards/px4/fmu-v4pro/src/init.c index dd3eae3e13e5..132db1f92424 100644 --- a/boards/px4/fmu-v4pro/src/init.c +++ b/boards/px4/fmu-v4pro/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -287,22 +288,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); @@ -321,7 +311,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 1); led_on(LED_RED); - return -ENODEV; } /* Default SPI1 to 1MHz and de-assert the known chip selects. */ @@ -337,7 +326,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 2); led_on(LED_RED); - return -ENODEV; } /* Default SPI2 to 12MHz and de-assert the known chip selects. @@ -355,7 +343,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi5) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 5); led_on(LED_RED); - return -ENODEV; } /* Default SPI5 to 1MHz and de-assert the known chip selects. */ @@ -370,7 +357,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi6) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", 6); led_on(LED_RED); - return -ENODEV; } /* Default SPI6 to 1MHz and de-assert the known chip selects. */ @@ -385,9 +371,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio) { led_on(LED_RED); - syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); } /* Now bind the SDIO interface to the MMC/SD driver */ @@ -396,7 +380,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ diff --git a/boards/px4/fmu-v4pro/src/mtd.cpp b/boards/px4/fmu-v4pro/src/mtd.cpp new file mode 100644 index 000000000000..9c0556f45b80 --- /dev/null +++ b/boards/px4/fmu-v4pro/src/mtd.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 +// KiB BS nB +static const px4_mft_device_t spi2 = { // FM25V01A on FMUM 16K + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi2, + .npart = 1, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &fmum_fram + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/px4/fmu-v4pro/test.px4board b/boards/px4/fmu-v4pro/test.px4board new file mode 100644 index 000000000000..27cedc328f4f --- /dev/null +++ b/boards/px4/fmu-v4pro/test.px4board @@ -0,0 +1,11 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board new file mode 100644 index 000000000000..274e5b45caf0 --- /dev/null +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_CRYPTO=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake deleted file mode 100644 index 50380155ee2c..000000000000 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ /dev/null @@ -1,139 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL ctrlalloc - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - angular_velocity_controller - attitude_estimator_q - battery_status - camera_feedback - commander - control_allocator - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5/cyphal.px4board b/boards/px4/fmu-v5/cyphal.px4board new file mode 100644 index 000000000000..d8754b8f34c3 --- /dev/null +++ b/boards/px4/fmu-v5/cyphal.px4board @@ -0,0 +1,7 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_DRIVERS_HEATER=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y +CONFIG_DRIVERS_CYPHAL=y diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake deleted file mode 100644 index f6e8ebcd52ed..000000000000 --- a/boards/px4/fmu-v5/debug.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL debug - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - #adc/ads1115 - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot - gps - #heater - #imu # all available imu drivers - #imu/bosch/bmi055 - #imu/invensense/icm20602 - imu/invensense/icm20689 - #irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - #pca9685 - #pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - #roboclaw - #rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - #uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #uuv_att_control - #uuv_pos_control - #vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v5/debug.px4board b/boards/px4/fmu-v5/debug.px4board new file mode 100644 index 000000000000..be936a6aeb49 --- /dev/null +++ b/boards/px4/fmu-v5/debug.px4board @@ -0,0 +1,52 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_COMMON_MAGNETOMETER=n +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_BATT_SMBUS=n +CONFIG_DRIVERS_DSHOT=n +CONFIG_DRIVERS_HEATER=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IMU_BOSCH_BMI055=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_POWER_MONITOR_INA226=n +CONFIG_DRIVERS_PWM_INPUT=n +CONFIG_DRIVERS_ROBOCLAW=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_SMART_BATTERY_BATMON=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_GYRO_CALIBRATION=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_GIMBAL=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y +CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y +CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y +CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y +CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +CONFIG_DRIVERS_MAGNETOMETER_RM3100=y +CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y +CONFIG_DRIVERS_TEST_PPM=y diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake deleted file mode 100644 index abc1cef9c66b..000000000000 --- a/boards/px4/fmu-v5/default.cmake +++ /dev/null @@ -1,137 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board new file mode 100644 index 000000000000..62b6690e6b21 --- /dev/null +++ b/boards/px4/fmu-v5/default.px4board @@ -0,0 +1,115 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_COMMON_HYGROMETERS=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v5/bootloader/px4_fmu-v5_bootloader.bin b/boards/px4/fmu-v5/extras/px4_fmu-v5_bootloader.bin similarity index 100% rename from boards/px4/fmu-v5/bootloader/px4_fmu-v5_bootloader.bin rename to boards/px4/fmu-v5/extras/px4_fmu-v5_bootloader.bin diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake deleted file mode 100644 index c1f354fb5b8c..000000000000 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ /dev/null @@ -1,104 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL fixedwing - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - load_mon - logger - mavlink - navigator - rc_update - sensors - temperature_compensation - vmount - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults index 98a8edb1012e..93bb336f8c5b 100644 --- a/boards/px4/fmu-v5/init/rc.board_defaults +++ b/boards/px4/fmu-v5/init/rc.board_defaults @@ -3,10 +3,15 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 18.1 +param set-default BAT2_V_DIV 18.1 -if ver hwtypecmp V550 V560 +param set-default BAT1_A_PER_V 36.367515152 +param set-default BAT2_A_PER_V 36.367515152 + +if ver hwtypecmp V550 V552 V560 V562 then - # CUAV V5+ (V550) and V5nano (V560) have 3 IMUs + # CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs # Multi-EKF (IMUs only) param set-default EKF2_MULTI_IMU 3 param set-default SENS_IMU_MODE 0 diff --git a/boards/px4/fmu-v5/init/rc.board_mavlink b/boards/px4/fmu-v5/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/px4/fmu-v5/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index 712e3b6d7e1d..e024e3225b2a 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -5,8 +5,14 @@ board_adc start -# Internal SPI bus ICM-20602 -icm20602 -s -R 2 -q start +if ver hwtypecmp V552 V562 +then + # Internal SPI bus ICM-42688-P + icm42688p -s -R 2 -q start +else + # Internal SPI bus ICM-20602 + icm20602 -s -R 2 -q start +fi # Internal SPI bus ICM-20689 icm20689 -s -R 2 start diff --git a/boards/px4/fmu-v5/lto.px4board b/boards/px4/fmu-v5/lto.px4board new file mode 100644 index 000000000000..cca9cdd73beb --- /dev/null +++ b/boards/px4/fmu-v5/lto.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LTO=y diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake deleted file mode 100644 index 64bbdf2b8491..000000000000 --- a/boards/px4/fmu-v5/multicopter.cmake +++ /dev/null @@ -1,112 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL multicopter - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - sensors - sih - temperature_compensation - vmount - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig new file mode 100644 index 000000000000..5ab78cad776d --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/cryptotest/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_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_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_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/debug/defconfig b/boards/px4/fmu-v5/nuttx-config/debug/defconfig index 03c4291a56e1..6b0e41bc6bdb 100644 --- a/boards/px4/fmu-v5/nuttx-config/debug/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/debug/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,8 +38,8 @@ CONFIG_ARMV7M_DCACHE=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -46,6 +47,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -54,25 +56,30 @@ CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DEBUG_BINFMT=y +CONFIG_DEBUG_BINFMT_ERROR=y +CONFIG_DEBUG_BINFMT_WARN=y +CONFIG_DEBUG_DMA=y +CONFIG_DEBUG_DMA_ERROR=y +CONFIG_DEBUG_DMA_WARN=y CONFIG_DEBUG_ERROR=y CONFIG_DEBUG_FEATURES=y CONFIG_DEBUG_FS=y -CONFIG_DEBUG_FS_ERROR=y CONFIG_DEBUG_FS_WARN=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_GPIO=y CONFIG_DEBUG_GPIO_ERROR=y CONFIG_DEBUG_GPIO_WARN=y CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_I2C=y -CONFIG_DEBUG_I2C_ERROR=y -CONFIG_DEBUG_I2C_WARN=y CONFIG_DEBUG_IRQ=y CONFIG_DEBUG_IRQ_ERROR=y CONFIG_DEBUG_IRQ_WARN=y CONFIG_DEBUG_LIB=y CONFIG_DEBUG_LIB_ERROR=y CONFIG_DEBUG_LIB_WARN=y +CONFIG_DEBUG_MEMCARD=y +CONFIG_DEBUG_MEMCARD_ERROR=y +CONFIG_DEBUG_MEMCARD_WARN=y CONFIG_DEBUG_MM=y CONFIG_DEBUG_MM_ERROR=y CONFIG_DEBUG_MM_WARN=y @@ -115,6 +122,7 @@ 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 @@ -123,11 +131,10 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_IDLETHREAD_STACKSIZE=1064 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -139,18 +146,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -160,21 +162,23 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_HPWORKSTACKSIZE=1300 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y CONFIG_SCHED_LPWORK=y @@ -184,7 +188,6 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y @@ -247,6 +250,9 @@ CONFIG_STM32F7_WWDG=y CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_RXDMA=y diff --git a/boards/px4/fmu-v5/nuttx-config/include/board.h b/boards/px4/fmu-v5/nuttx-config/include/board.h index 26b14e1360bb..de1fb2099d49 100644 --- a/boards/px4/fmu-v5/nuttx-config/include/board.h +++ b/boards/px4/fmu-v5/nuttx-config/include/board.h @@ -397,27 +397,15 @@ #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_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ -#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7) -#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8) - #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) - /* SDMMC1 * * VDD 3.3 diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index eb14169b5fa0..e194b354065f 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -38,6 +39,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -45,13 +47,13 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y CONFIG_DEBUG_SYMBOLS=y @@ -60,6 +62,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -71,6 +74,7 @@ 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 @@ -80,10 +84,8 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -95,18 +97,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -116,16 +113,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -140,8 +139,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/px4/fmu-v5/nuttx-config/optimized/defconfig b/boards/px4/fmu-v5/nuttx-config/optimized/defconfig deleted file mode 100644 index 2b346483268a..000000000000 --- a/boards/px4/fmu-v5/nuttx-config/optimized/defconfig +++ /dev/null @@ -1,245 +0,0 @@ -# -# 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_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32f7" -CONFIG_ARCH_CHIP_STM32F765II=y -CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -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_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=22114 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0032 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_CUSTOMOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_OPTLEVEL="-O3" -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=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_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_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y -CONFIG_NSH_DISABLE_TELNETD=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_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_MODE=y -CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -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_STM32F7_ADC1=y -CONFIG_STM32F7_BBSRAM=y -CONFIG_STM32F7_BBSRAM_FILES=5 -CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_DMA1=y -CONFIG_STM32F7_DMA2=y -CONFIG_STM32F7_DMACAPABLE=y -CONFIG_STM32F7_FLOWCONTROL_BROKEN=y -CONFIG_STM32F7_I2C1=y -CONFIG_STM32F7_I2C2=y -CONFIG_STM32F7_I2C3=y -CONFIG_STM32F7_I2C4=y -CONFIG_STM32F7_I2C_DYNTIMEO=y -CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32F7_OTGFS=y -CONFIG_STM32F7_PROGMEM=y -CONFIG_STM32F7_PWR=y -CONFIG_STM32F7_RTC=y -CONFIG_STM32F7_RTC_HSECLOCK=y -CONFIG_STM32F7_RTC_MAGIC_REG=1 -CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC1=y -CONFIG_STM32F7_SDMMC_DMA=y -CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32F7_SPI1=y -CONFIG_STM32F7_SPI1_DMA=y -CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI2=y -CONFIG_STM32F7_SPI4=y -CONFIG_STM32F7_SPI5=y -CONFIG_STM32F7_SPI6=y -CONFIG_STM32F7_SPI_DMA=y -CONFIG_STM32F7_SPI_DMATHRESHOLD=8 -CONFIG_STM32F7_TIM10=y -CONFIG_STM32F7_TIM11=y -CONFIG_STM32F7_UART4=y -CONFIG_STM32F7_UART7=y -CONFIG_STM32F7_UART8=y -CONFIG_STM32F7_USART1=y -CONFIG_STM32F7_USART2=y -CONFIG_STM32F7_USART3=y -CONFIG_STM32F7_USART6=y -CONFIG_STM32F7_USART_BREAKS=y -CONFIG_STM32F7_USART_INVERT=y -CONFIG_STM32F7_USART_SINGLEWIRE=y -CONFIG_STM32F7_USART_SWAP=y -CONFIG_STM32F7_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=1500 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_RXDMA=y -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1500 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=3000 -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_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/protected/defconfig b/boards/px4/fmu-v5/nuttx-config/protected/defconfig new file mode 100644 index 000000000000..bec6725e7750 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/protected/defconfig @@ -0,0 +1,250 @@ +# +# 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_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_ARM_MPU_NREGIONS=8 +CONFIG_BOARDCTL_IOCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILD_PROTECTED=y +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_USRWORK=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_KERNEL_HEAPSIZE=131072 +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_NUTTX_USERSPACE=0x08100000 +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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_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_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYS_RESERVED=9 +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="px4_entry" diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/kernel-space.ld b/boards/px4/fmu-v5/nuttx-config/scripts/kernel-space.ld new file mode 100644 index 000000000000..8ad6e7bf44a5 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/scripts/kernel-space.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * kernel-space.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. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ + +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) +/* + * TODO: Fill in the signature location into TOC from user-space elf +EXTERN(_main_toc) +*/ + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + /* + *(.main_toc) + */ + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > kflash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > kflash + + + .ARM.extab : { + *(.ARM.extab*) + } > kflash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > kflash + + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > ksram AT > kflash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > ksram + + /* 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) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ksram AT > kflash + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/memory.ld b/boards/px4/fmu-v5/nuttx-config/scripts/memory.ld new file mode 100644 index 000000000000..23e536e9c5c0 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/scripts/memory.ld @@ -0,0 +1,98 @@ +/**************************************************************************** + * scripts/memory.ld + * + * Copyright (C) 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. + * + ****************************************************************************/ + +/* The STM32F765IIT6 has 2048 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F765IIT6, 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 on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option byes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F765IIT6 will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F765IIT6 also has 512 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 128 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 368 KiB of SRAM1 beginning at address 0x2002:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2007:c000 + * + * 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. + * + * Bootloader reserves the first 32K bank (2 Mbytes Flash memory single bank) + * organization (256 bits read width) + */ + +MEMORY +{ + /* ITCM boot address */ + + itcm (rwx) : ORIGIN = 0x00208000, LENGTH = 2048K-32K + + /* 2048KB FLASH, bootloader reserves the first 32kb */ + + kflash (rx) : ORIGIN = 0x08008000, LENGTH = 1024K-32K + uflash (rx) : ORIGIN = 0x08100000, LENGTH = 1024K + + /* ITCM RAM */ + + itcm_ram (rwx) : ORIGIN = 0x00000000, LENGTH = 16K + + /* DTCM SRAM */ + + dtcm_ram (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + + /* 368KB of contiguous SRAM1 */ + + ksram (rwx) : ORIGIN = 0x20020000, LENGTH = 128K + usram (rwx) : ORIGIN = 0x20040000, LENGTH = 368K-128K + + /* 16KB of SRAM2 */ + + sram2 (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K +} diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/user-space.ld b/boards/px4/fmu-v5/nuttx-config/scripts/user-space.ld new file mode 100644 index 000000000000..e19ddea75607 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/scripts/user-space.ld @@ -0,0 +1,124 @@ +/**************************************************************************** + * user-space.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. + * + ****************************************************************************/ + +/* NOTE: This depends on the memory.ld script having been included prior to + * this script. + */ +EXTERN(userspace) +OUTPUT_ARCH(arm) +SECTIONS +{ + .userspace : { + *(.userspace) + } > uflash + + .text : { + _stext = ABSOLUTE(.); + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > uflash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > uflash + + + .ARM.extab : { + *(.ARM.extab*) + } > uflash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > uflash + + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > usram AT > uflash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + /* Kernel heap start at _ebss, make _ebss MPU-friendly aligned */ + . = ALIGN(0x1000); + _ebss = ABSOLUTE(.); + } > usram + + /* 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) } + + /* Start of the image signature. This + * has to be in the end of the image + */ + .signature : { + _boot_signature = ALIGN(4); + } > uflash +} diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 0d0002efcf1a..d93a7f314956 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -39,6 +40,7 @@ CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -46,6 +48,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -72,6 +75,7 @@ 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 @@ -80,11 +84,10 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_IDLETHREAD_STACKSIZE=864 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -96,18 +99,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,16 +115,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -141,8 +141,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/px4/fmu-v5/nuttx-config/test/defconfig b/boards/px4/fmu-v5/nuttx-config/test/defconfig new file mode 100644 index 000000000000..e5e893f1cdfa --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/test/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_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON 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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=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_DISABLE_MQUEUE=y +CONFIG_EXAMPLES_CALIB_UDELAY=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_LIBC_LONG_LONG=y +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=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_TELNETD=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_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_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig b/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig index ddcdb2b52a29..7695c6402983 100644 --- a/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/uavcanv1/defconfig @@ -20,11 +20,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -38,15 +39,17 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y -CONFIG_CAN=y CONFIG_CAN_EXTID=y +CONFIG_CAN_FIFOSIZE=32 CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -73,6 +76,7 @@ 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 @@ -85,7 +89,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -97,18 +100,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -118,16 +116,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -142,8 +142,7 @@ CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake deleted file mode 100644 index 554320c4c530..000000000000 --- a/boards/px4/fmu-v5/optimized.cmake +++ /dev/null @@ -1,128 +0,0 @@ - -# set Release for -O3 -set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Build type" FORCE) -add_compile_options(-Wno-error=array-bounds) - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL optimized - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - #UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - #lights/rgbled_pwm - #magnetometer # all available magnetometer drivers - magnetometer/isentek/ist8310 - optical_flow # all available optical flow drivers - #osd - #pca9685 - #pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - #roboclaw - #rpm - safety_button - #telemetry # all available telemetry drivers - test_ppm - tone_alarm - #uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - #vmount - vtol_att_control - SYSTEMCMDS - #bl_update - dmesg - dumpfile - #esc_calib - #gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board new file mode 100644 index 000000000000..d51a7314d5a8 --- /dev/null +++ b/boards/px4/fmu-v5/protected.px4board @@ -0,0 +1,44 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_COMMON_DISTANCE_SENSOR=n +CONFIG_COMMON_OPTICAL_FLOW=n +CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_ROBOCLAW=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL_L1=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_DMESG=n +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_GPIO=n +CONFIG_SYSTEMCMDS_I2CDETECT=n +CONFIG_SYSTEMCMDS_LED_CONTROL=n +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_MTD=n +CONFIG_SYSTEMCMDS_NSHTERM=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=n +CONFIG_SYSTEMCMDS_USB_CONNECTED=n +CONFIG_BOARD_PROTECTED=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake deleted file mode 100644 index 78758ae35139..000000000000 --- a/boards/px4/fmu-v5/rover.cmake +++ /dev/null @@ -1,90 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL rover - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - gps - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - lights/rgbled - lights/rgbled_ncp5623c - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - pca9685 - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - battery_status - camera_feedback - commander - dataman - ekf2 - events - land_detector - load_mon - logger - mavlink - navigator - rc_update - rover_pos_control - sensors - temperature_compensation - vmount - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake deleted file mode 100644 index 280510bcb474..000000000000 --- a/boards/px4/fmu-v5/rtps.cmake +++ /dev/null @@ -1,128 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - power_monitor/ina226 - protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5/rtps.px4board b/boards/px4/fmu-v5/rtps.px4board new file mode 100644 index 000000000000..fa4fe4f07aeb --- /dev/null +++ b/boards/px4/fmu-v5/rtps.px4board @@ -0,0 +1,4 @@ +CONFIG_DRIVERS_HEATER=n +CONFIG_DRIVERS_OSD=n +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5/src/CMakeLists.txt b/boards/px4/fmu-v5/src/CMakeLists.txt index d00f86e35636..f900cbf8be4b 100644 --- a/boards/px4/fmu-v5/src/CMakeLists.txt +++ b/boards/px4/fmu-v5/src/CMakeLists.txt @@ -31,19 +31,23 @@ # ############################################################################ +add_library(board_bus_info + i2c.cpp + spi.cpp +) +add_dependencies(board_bus_info nuttx_context) + add_library(drivers_board can.c - i2c.cpp init.c led.c manifest.c sdio.c - spi.cpp timer_config.cpp usb.c toc.c ) -add_dependencies(drivers_board arch_board_hw_info) +add_dependencies(drivers_board nuttx_context) target_link_libraries(drivers_board PRIVATE @@ -52,5 +56,16 @@ target_link_libraries(drivers_board drivers__led # drv_led_start nuttx_arch # sdio nuttx_drivers # sdio - px4_layer ) + +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(drivers_board PRIVATE px4_kernel_layer) +else() + target_link_libraries(drivers_board PRIVATE px4_layer board_bus_info) +endif() + +add_library(drivers_userspace + stm32_userspace.c +) + +add_dependencies(drivers_userspace nuttx_context) diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index 6cfdf8fa60bf..0d9bb9041b90 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -49,6 +49,11 @@ #include +#if !defined(CONFIG_BUILD_FLAT) +#include +#include +#endif + /**************************************************************************************************** * Definitions ****************************************************************************************************/ @@ -173,18 +178,10 @@ (1 << ADC1_SPARE_1_CHANNEL)) #endif -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* 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_VERSIONING #define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) @@ -194,6 +191,15 @@ #define HW_INFO_INIT {'V','5','x', 'x',0} #define HW_INFO_INIT_VER 2 #define HW_INFO_INIT_REV 3 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 +#define V500 HW_VER_REV(0x0,0x0) // FMUV5, Rev 0 +#define V515 HW_VER_REV(0x1,0x5) // CUAV V5, Rev 5 +#define V540 HW_VER_REV(0x4,0x0) // HolyBro mini no can 2,3, Rev 0 +#define V550 HW_VER_REV(0x5,0x0) // CUAV V5+, Rev 0 +#define V552 HW_VER_REV(0x5,0x2) // CUAV V5+ ICM42688P, Rev 2 +#define V560 HW_VER_REV(0x6,0x0) // CUAV V5nano with can 2, Rev 0 +#define V562 HW_VER_REV(0x6,0x2) // CUAV V5nano ICM42688P, Rev 2 + /* CAN Silence * * Silent mode control \ ESC Mux select @@ -209,22 +215,8 @@ * PWM in future */ #define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) -/* PWM Capture - * - * 3 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PA5 : TIM2_CH1 - * FMU_CAP2 : PB3 : TIM2_CH2 - * FMU_CAP3 : PB11 : TIM2_CH4 - */ -#define GPIO_TIM2_CH1_IN /* PA5 T22C1 FMU_CAP1 */ GPIO_TIM2_CH1IN_3 -#define GPIO_TIM2_CH2_IN /* PB3 T22C2 FMU_CAP2 */ GPIO_TIM2_CH2IN_2 -#define GPIO_TIM2_CH4_IN /* PB11 T22C4 FMU_CAP3 */ GPIO_TIM2_CH4IN_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 3 /* PI0 is nARMED * The GPIO will be set as input while not armed HW will have external HW Pull UP. @@ -233,12 +225,30 @@ #define GPIO_nARMED_INIT /* PI0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTI|GPIO_PIN0) #define GPIO_nARMED /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) +/* For protected build, define the LOCKOUT_STATE macros as function calls */ +#ifdef CONFIG_BUILD_FLAT #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) +#else +static inline void board_indicate_external_lockout_state(bool enable) +{ + platformioclockoutstate_t state = {enable}; + boardctl(PLATFORMIOCINDICATELOCKOUT, (uintptr_t)&state); +} + +static inline bool board_get_external_lockout_state(void) +{ + platformioclockoutstate_t state = {false}; + boardctl(PLATFORMIOCGETLOCKOUT, (uintptr_t)&state); + return state.enabled; +} +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) board_indicate_external_lockout_state(enabled) +#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() board_get_external_lockout_state() +#endif /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 11 #define BOARD_HAS_LED_PWM 1 #define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 @@ -262,7 +272,7 @@ #define GPIO_nVDD_5V_PERIPH_EN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) #define GPIO_nVDD_5V_PERIPH_OC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) #define GPIO_nVDD_5V_HIPOWER_EN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) -#define GPIO_nVDD_5V_HIPOWER_OC /* PG13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_nVDD_5V_HIPOWER_OC /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_VDD_3V3_SPEKTRUM_PASSIVE /* PE4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN4) #define GPIO_VDD_5V_RC_EN /* PG5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN5) @@ -307,19 +317,7 @@ #define RC_SERIAL_PORT "/dev/ttyS4" #define RC_SERIAL_SINGLEWIRE - -/* Input Capture Channels. */ -#define INPUT_CAP1_TIMER 2 -#define INPUT_CAP1_CHANNEL /* T4C1 */ 1 -#define GPIO_INPUT_CAP1 /* PA5 */ GPIO_TIM2_CH1_IN - -#define INPUT_CAP2_TIMER 2 -#define INPUT_CAP2_CHANNEL /* T4C2 */ 2 -#define GPIO_INPUT_CAP2 /* PB3 */ GPIO_TIM2_CH2_IN - -#define INPUT_CAP3_TIMER 2 -#define INPUT_CAP3_CHANNEL /* T4C4 */ 4 -#define GPIO_INPUT_CAP3 /* PB11 */ GPIO_TIM2_CH4_IN +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -424,7 +422,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -462,14 +459,21 @@ GPIO_RSSI_IN_INIT, \ GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \ GPIO_SAFETY_SWITCH_IN, \ - GPIO_nARMED_INIT \ + GPIO_nARMED_INIT, \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \ } #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_NUM_IO_TIMERS 5 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index 60d4c756e825..cfec81bae6c4 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -254,22 +255,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) } #if defined(SERIAL_HAVE_RXDMA) - /* set up the serial DMA polling */ + // 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; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - struct timespec ts; - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); #endif /* initial LED state */ @@ -287,7 +275,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/px4/fmu-v5/src/manifest.c b/boards/px4/fmu-v5/src/manifest.c index 07fe07e4fd7c..3e131f9adaaf 100644 --- a/boards/px4/fmu-v5/src/manifest.c +++ b/boards/px4/fmu-v5/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -155,11 +157,13 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = { static px4_hw_mft_list_entry_t mft_lists[] = { - {0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, - {0x0105, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1 - {0x0500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5 - {0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // HolyBro mini no can 2,3 - {0x0600, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // CUAV V5nano R:0 V:6 with can 2 + {V500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, + {V515, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1 + {V540, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // HolyBro mini no can 2,3 + {V550, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5 + {V552, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:2 V:5 ICM42688P + {V560, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // CUAV V5nano R:0 V:6 with can 2 + {V562, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5nano R:2 V:6 ICM42688P }; /************************************************************************************ @@ -193,7 +197,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v5/src/spi.cpp b/boards/px4/fmu-v5/src/spi.cpp index 15d9751930b3..8a3f8c9b2999 100644 --- a/boards/px4/fmu-v5/src/spi.cpp +++ b/boards/px4/fmu-v5/src/spi.cpp @@ -35,29 +35,81 @@ #include #include -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), - initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), - }, {GPIO::PortE, GPIO::Pin3}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V500, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), }), - initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), - }), - initSPIBusExternal(SPI::Bus::SPI5, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11}) + + initSPIHWVersion(V552, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + + initSPIHWVersion(V562, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}), + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8}) + }), }), }; -static constexpr bool unused = validateSPIConfig(px4_spi_buses); +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v5/src/stm32_userspace.c b/boards/px4/fmu-v5/src/stm32_userspace.c new file mode 100644 index 000000000000..2c52f986bbad --- /dev/null +++ b/boards/px4/fmu-v5/src/stm32_userspace.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * boards/px4/fmu-v5/src/stm32_userspace.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include + +#if !defined(CONFIG_BUILD_FLAT) && !defined(__KERNEL__) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +#ifndef CONFIG_NUTTX_USERSPACE +# error "CONFIG_NUTTX_USERSPACE not defined" +#endif + +#if CONFIG_NUTTX_USERSPACE != 0x08100000 +# error "CONFIG_NUTTX_USERSPACE must be 0x08100000 to match memory.ld" +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* These 'addresses' of these values are setup by the linker script. They are + * not actual uint32_t storage locations! They are only used meaningfully in + * the following way: + * + * - The linker script defines, for example, the symbol_sdata. + * - The declaration extern uint32_t _sdata; makes C happy. C will believe + * that the value _sdata is the address of a uint32_t variable _data (it + * is not!). + * - We can recover the linker value then by simply taking the address of + * of _data. like: uint32_t *pdata = &_sdata; + */ + +extern uint32_t _stext; /* Start of .text */ +extern uint32_t _etext; /* End_1 of .text + .rodata */ +extern const uint32_t _eronly; /* End+1 of read only section (.text + .rodata) */ +extern uint32_t _sdata; /* Start of .data */ +extern uint32_t _edata; /* End+1 of .data */ +extern uint32_t _sbss; /* Start of .bss */ +extern uint32_t _ebss; /* End+1 of .bss */ + +/* This is the user space entry point */ + +int CONFIG_USER_ENTRYPOINT(int argc, char *argv[]); +int nsh_main(int argc, char *argv[]); + +const struct userspace_s userspace __attribute__((section(".userspace"))) = { + /* General memory map */ + + .us_entrypoint = (main_t)CONFIG_USER_ENTRYPOINT, + .us_textstart = (uintptr_t) &_stext, + .us_textend = (uintptr_t) &_etext, + .us_datasource = (uintptr_t) &_eronly, + .us_datastart = (uintptr_t) &_sdata, + .us_dataend = (uintptr_t) &_edata, + .us_bssstart = (uintptr_t) &_sbss, + .us_bssend = (uintptr_t) &_ebss, + + /* Memory manager heap structure */ + + .us_heap = &g_mmheap, + + /* Task/thread startup routines */ + + .task_startup = nxtask_startup, + + /* Signal handler trampoline */ + + .signal_handler = up_signal_handler, + + /* User-space work queue support (declared in include/nuttx/wqueue.h) */ + +#ifdef CONFIG_LIB_USRWORK + .work_usrstart = work_usrstart, +#endif +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void px4_userspace_init(void); + +int CONFIG_USER_ENTRYPOINT(int argc, char *argv[]) +{ + +#ifdef CONFIG_NSH_ARCHINIT +#error CONFIG_NSH_ARCHINIT must not be defined! +#endif + + boardctl(BOARDIOC_INIT, 0); + + px4_userspace_init(); + + return nsh_main(argc, argv); +} + +#endif /* !CONFIG_BUILD_FLAT && !__KERNEL__ */ diff --git a/boards/px4/fmu-v5/src/timer_config.cpp b/boards/px4/fmu-v5/src/timer_config.cpp index c6e21851f3b1..8bebdca77312 100644 --- a/boards/px4/fmu-v5/src/timer_config.cpp +++ b/boards/px4/fmu-v5/src/timer_config.cpp @@ -50,6 +50,9 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin5}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortB, GPIO::Pin3}), + initIOTimerChannelCapture(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake deleted file mode 100644 index 49afc1556568..000000000000 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL stackcheck - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - #UAVCAN_INTERFACES 2 - CONSTRAINED_FLASH - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - #camera_capture - #camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - #imu/analog_devices/adis16448 - #imu/adis16477 - #imu/adis16497 - #imu/bosch/bmi055 - #imu/invensense/icm20602 - imu/invensense/icm20689 - #irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - #pca9685 - #power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - #roboclaw - #rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm - tone_alarm - #uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - #vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gps - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board new file mode 100644 index 000000000000..2ca91e87ca41 --- /dev/null +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -0,0 +1,42 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_HYGROMETERS=n +CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_BATT_SMBUS=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_DRIVERS_HEATER=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_POWER_MONITOR_INA226=n +CONFIG_DRIVERS_PWM_INPUT=n +CONFIG_DRIVERS_PWM_OUT_SIM=n +CONFIG_DRIVERS_ROBOCLAW=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_SMART_BATTERY_BATMON=n +CONFIG_DRIVERS_UAVCAN=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_CAMERA_FEEDBACK=n +CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_GIMBAL=n +CONFIG_MODULES_GYRO_CALIBRATION=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_TESTING=y diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board new file mode 100644 index 000000000000..5279a489dc80 --- /dev/null +++ b/boards/px4/fmu-v5/test.px4board @@ -0,0 +1,14 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake deleted file mode 100644 index f943c6a97cd9..000000000000 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ /dev/null @@ -1,139 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL uavcanv0periph - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - #TESTING - UAVCAN_INTERFACES 2 - UAVCAN_PERIPHERALS - cuav_can-gps-v1_default - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - #adc/ads1115 - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - #imu/adis16448 - #imu/adis16477 - #imu/adis16497 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - #irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - #pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - #roboclaw - #rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - #esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - #gyro_fft - land_detector - landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #bl_update - dmesg - #dumpfile - esc_calib - #gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board new file mode 100644 index 000000000000..f7565e20c9ad --- /dev/null +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -0,0 +1,38 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n +CONFIG_COMMON_HYGROMETERS=n +CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_HEATER=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_OSD=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_PWM_INPUT=n +CONFIG_DRIVERS_ROBOCLAW=n +CONFIG_DRIVERS_RPM=n +CONFIG_DRIVERS_SMART_BATTERY_BATMON=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_ESC_BATTERY=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n +CONFIG_MODULES_UUV_ATT_CONTROL=n +CONFIG_MODULES_UUV_POS_CONTROL=n +CONFIG_SYSTEMCMDS_BL_UPDATE=n +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_GPIO=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=n +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n +CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default" +CONFIG_DRIVERS_BAROMETER_MS5611=y diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake deleted file mode 100644 index 619fae5e90ab..000000000000 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL uavcanv1 - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - #TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi055 - imu/invensense/icm20602 - imu/invensense/icm20689 - irlock - lights # all available light drivers - lights/rgbled_pwm - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - #test_ppm - tone_alarm - #uavcan # legacy v0 - uavcan_v1 - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake deleted file mode 100644 index c653a9296939..000000000000 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5x - LABEL base_phy_DP83848C - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - ETHERNET - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm42688p - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - netman - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake deleted file mode 100644 index 06a0e5349d99..000000000000 --- a/boards/px4/fmu-v5x/default.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5x - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - ETHERNET - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm42688p - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - netman - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board new file mode 100644 index 000000000000..135cbc3e609d --- /dev/null +++ b/boards/px4/fmu-v5x/default.px4board @@ -0,0 +1,116 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v5x/bootloader/px4_fmu-v5x_bootloader.bin b/boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin similarity index 100% rename from boards/px4/fmu-v5x/bootloader/px4_fmu-v5x_bootloader.bin rename to boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 9c542a77300e..a7d844a36632 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -3,4 +3,17 @@ # 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 1 + safety_button start diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index 07ed2047b404..c0dbc024035b 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -1,13 +1,10 @@ #!/bin/sh # -# PX4 FMUv5X specific board MAVLink startup script. +# board specific MAVLink startup script. #------------------------------------------------------------------------------ -if ver hwtypecmp V5Xa0 V5X91 V5Xa1 +if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 V5X80 V5X81 then -# Start MAVLink on the UART connected to the mission computer -mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z -else -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 + # Start MAVLink on the UART connected to the mission computer + mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z fi diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 633784ecb826..1dfa20838438 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -2,19 +2,65 @@ # # PX4 FMUv5X specific board sensors init #------------------------------------------------------------------------------ -board_adc start -# Start Digital power monitors -ina226 -X -b 1 -t 1 -k start -ina226 -X -b 2 -t 2 -k start +set HAVE_PM2 yes -if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 +if ver hwtypecmp V5X50 V5X51 V5X52 +then + set HAVE_PM2 no +fi +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 + + if [ $HAVE_PM2 = yes ] + then + ina226 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA228 1 +then + # Start Digital power monitors + ina228 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina228 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA238 1 +then + # Start Digital power monitors + ina238 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina238 -X -b 2 -t 2 -k start + fi +fi + +if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2 then #SKYNODE base fmu board orientation - # Internal SPI BMI088 - bmi088 -A -R 2 -s start - bmi088 -G -R 2 -s start + if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 + then + # Internal SPI BMI088 + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start + else + # Internal SPI bus ICM20649 + icm20649 -s -R 4 start + fi # Internal SPI bus ICM42688p icm42688p -R 4 -s start @@ -28,9 +74,15 @@ then else #FMUv5Xbase board orientation - # Internal SPI BMI088 - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + if ver hwtypecmp V5X00 V5X01 + then + # Internal SPI BMI088 + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + else + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start + fi # Internal SPI bus ICM42688p icm42688p -R 6 -s start @@ -43,17 +95,21 @@ else fi +# 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 -a 0x77 start - if ver hwtypecmp V5X91 V5Xa1 + if ver hwtypecmp V5X00 V5X90 V5Xa0 then - bmp388 -X -b 2 start - else bmp388 -I start + else + bmp388 -X -b 2 start fi fi +unset HAVE_PM2 \ No newline at end of file diff --git a/boards/px4/fmu-v5x/nuttx-config/base_phy_DP83848C/defconfig b/boards/px4/fmu-v5x/nuttx-config/base_phy_DP83848C/defconfig deleted file mode 100644 index 840922f5e3dc..000000000000 --- a/boards/px4/fmu-v5x/nuttx-config/base_phy_DP83848C/defconfig +++ /dev/null @@ -1,293 +0,0 @@ -# -# 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_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_ARP is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32f7" -CONFIG_ARCH_CHIP_STM32F765II=y -CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -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_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=22114 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0033 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x3185 -CONFIG_CDCACM_VENDORSTR="Auterion" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_GPIO=y -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_ETH0_PHY_DP83848C=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_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -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_IOB_NBUFFERS=24 -CONFIG_IOB_THROTTLE=0 -CONFIG_IPCFG_BINARY=y -CONFIG_IPCFG_CHARDEV=y -CONFIG_IPCFG_PATH="/fs/mtd_net" -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NET=y -CONFIG_NETDB_DNSCLIENT=y -CONFIG_NETDB_DNSCLIENT_ENTRIES=8 -CONFIG_NETDB_DNSSERVER_NOADDR=y -CONFIG_NETDEV_PHY_IOCTL=y -CONFIG_NETINIT_DHCPC=y -CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE -CONFIG_NETINIT_MONITOR=y -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_ICMP=y -CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=y -CONFIG_NET_SOLINGER=y -CONFIG_NET_TCP=y -CONFIG_NET_TCPBACKLOG=y -CONFIG_NET_TCP_WRITE_BUFFERS=y -CONFIG_NET_UDP=y -CONFIG_NET_UDP_CHECKSUMS=y -CONFIG_NET_UDP_WRITE_BUFFERS=y -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=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_TELNET=y -CONFIG_NSH_TELNET_LOGIN=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -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_STM32F7_ADC1=y -CONFIG_STM32F7_ADC3=y -CONFIG_STM32F7_BBSRAM=y -CONFIG_STM32F7_BBSRAM_FILES=5 -CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_DMA1=y -CONFIG_STM32F7_DMA2=y -CONFIG_STM32F7_DMACAPABLE=y -CONFIG_STM32F7_ETHMAC=y -CONFIG_STM32F7_FLOWCONTROL_BROKEN=y -CONFIG_STM32F7_I2C1=y -CONFIG_STM32F7_I2C2=y -CONFIG_STM32F7_I2C3=y -CONFIG_STM32F7_I2C4=y -CONFIG_STM32F7_I2C_DYNTIMEO=y -CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32F7_OTGFS=y -CONFIG_STM32F7_PHYADDR=0 -CONFIG_STM32F7_PHYSR=31 -CONFIG_STM32F7_PHYSR_100MBPS=0x8 -CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10 -CONFIG_STM32F7_PHYSR_MODE=0x10 -CONFIG_STM32F7_PHYSR_SPEED=0x8 -CONFIG_STM32F7_PHY_POLLING=y -CONFIG_STM32F7_PROGMEM=y -CONFIG_STM32F7_PWR=y -CONFIG_STM32F7_RTC=y -CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y -CONFIG_STM32F7_RTC_MAGIC_REG=1 -CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC2=y -CONFIG_STM32F7_SDMMC_DMA=y -CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32F7_SPI1=y -CONFIG_STM32F7_SPI1_DMA=y -CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI2=y -CONFIG_STM32F7_SPI2_DMA=y -CONFIG_STM32F7_SPI2_DMA_BUFFER=4096 -CONFIG_STM32F7_SPI3=y -CONFIG_STM32F7_SPI3_DMA=y -CONFIG_STM32F7_SPI3_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI5=y -CONFIG_STM32F7_SPI6=y -CONFIG_STM32F7_SPI_DMA=y -CONFIG_STM32F7_SPI_DMATHRESHOLD=8 -CONFIG_STM32F7_TIM10=y -CONFIG_STM32F7_TIM11=y -CONFIG_STM32F7_TIM3=y -CONFIG_STM32F7_TIM9=y -CONFIG_STM32F7_UART4=y -CONFIG_STM32F7_UART5=y -CONFIG_STM32F7_UART7=y -CONFIG_STM32F7_UART8=y -CONFIG_STM32F7_USART1=y -CONFIG_STM32F7_USART2=y -CONFIG_STM32F7_USART3=y -CONFIG_STM32F7_USART6=y -CONFIG_STM32F7_USART_BREAKS=y -CONFIG_STM32F7_USART_INVERT=y -CONFIG_STM32F7_USART_SINGLEWIRE=y -CONFIG_STM32F7_USART_SWAP=y -CONFIG_STM32F7_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_DHCPC_RENEW=y -CONFIG_SYSTEM_NSH=y -CONFIG_SYSTEM_PING=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=3000 -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_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_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=3000 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_RXBUFSIZE=180 -CONFIG_USART3_SERIAL_CONSOLE=y -CONFIG_USART3_TXBUFSIZE=1500 -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_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5x/nuttx-config/include/board.h b/boards/px4/fmu-v5x/nuttx-config/include/board.h index e92d68da9b5e..724ecd7f53c7 100644 --- a/boards/px4/fmu-v5x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v5x/nuttx-config/include/board.h @@ -394,27 +394,15 @@ #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ #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_PIN8) -#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_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_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ -#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7) -#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8) - #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 diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 9566fef0374f..e36b4ef4e3bc 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5x/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32f7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=22114 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0033 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -59,6 +62,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_GPIO=y CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y CONFIG_ETH0_PHY_LAN8742A=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -72,6 +76,7 @@ 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 @@ -89,7 +94,6 @@ CONFIG_IPCFG_PATH="/fs/mtd_net" CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -116,26 +120,22 @@ CONFIG_NETUTILS_TELNETD=y CONFIG_NET_ARP_IPIN=y CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y +CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=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_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -146,16 +146,18 @@ CONFIG_NSH_STRERROR=y CONFIG_NSH_TELNET=y CONFIG_NSH_TELNET_LOGIN=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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -169,8 +171,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y diff --git a/boards/px4/fmu-v5x/nuttx-config/test/defconfig b/boards/px4/fmu-v5x/nuttx-config/test/defconfig new file mode 100644 index 000000000000..e4c812c34be2 --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/test/defconfig @@ -0,0 +1,297 @@ +# +# 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_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_ARP is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5x/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0033 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_ETH0_PHY_LAN8742A=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_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_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_BINARY=y +CONFIG_IPCFG_CHARDEV=y +CONFIG_IPCFG_PATH="/fs/mtd_net" +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSCLIENT_ENTRIES=8 +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DHCPC=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_MONITOR=y +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_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_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +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_TELNET=y +CONFIG_NSH_TELNET_LOGIN=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_STM32F7_ADC1=y +CONFIG_STM32F7_ADC3=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_ETHMAC=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PHYADDR=0 +CONFIG_STM32F7_PHYSR=31 +CONFIG_STM32F7_PHYSR_100MBPS=0x8 +CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10 +CONFIG_STM32F7_PHYSR_MODE=0x10 +CONFIG_STM32F7_PHYSR_SPEED=0x8 +CONFIG_STM32F7_PHY_POLLING=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC2=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI2_DMA=y +CONFIG_STM32F7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_SPI3_DMA=y +CONFIG_STM32F7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM9=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART5=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_DHCPC_RENEW=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TESTING_OSTEST=y +CONFIG_TESTING_OSTEST_FPUTESTDISABLE=y +CONFIG_TESTING_OSTEST_STACKSIZE=10240 +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=3000 +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_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_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5x/rtps.px4board b/boards/px4/fmu-v5x/rtps.px4board new file mode 100644 index 000000000000..64ee923278a7 --- /dev/null +++ b/boards/px4/fmu-v5x/rtps.px4board @@ -0,0 +1,15 @@ +CONFIG_COMMON_BAROMETERS=n +CONFIG_COMMON_TELEMETRY=n +CONFIG_DRIVERS_OSD=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n +CONFIG_SYSTEMCMDS_DUMPFILE=n +CONFIG_SYSTEMCMDS_MOTOR_TEST=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_MODULES_MICRODDS_CLIENT=y +CONFIG_MODULES_MICRORTPS_BRIDGE=y diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 3dbc72879f6e..684b11736eda 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -43,10 +43,12 @@ add_library(drivers_board timer_config.cpp usb.c ) -add_dependencies(drivers_board arch_board_hw_info platform_gpio_mcp23009) + +add_dependencies(drivers_board platform_gpio_mcp23009) target_link_libraries(drivers_board PRIVATE + arch_io_pins arch_spi arch_board_hw_info drivers__led # drv_led_start diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 64bcd8e4fbaa..d1055b7692b0 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -185,25 +185,29 @@ #define HW_INFO_INIT {'V','5','X','x', 'x',0} #define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ #define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 +// Base FMUM +#define V5X00 HW_VER_REV(0x0,0x0) // FMUV5X, Rev 0 +#define V5X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 +#define V5X01 HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388, Rev 1 +#define V5X02 HW_VER_REV(0x0,0x2) // FMUV5X, Rev 2 +#define V5X50 HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini Rev 0 +#define V5X51 HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini Rev 1 +#define V5X52 HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini Rev 2 +#define V5X90 HW_VER_REV(0x9,0x0) // NO USB, Rev 0 +#define V5X91 HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388, Rev 1 +#define V5X92 HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388, Rev 2 +#define V5Xa0 HW_VER_REV(0xa,0x0) // NO USB (Q), Rev 0 +#define V5Xa1 HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 +#define V5Xa2 HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2 + +#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) - -/* PWM Capture - * - * 1 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PI0 : TIM5_CH4 - */ - -#define GPIO_TIM5_CH4IN /* PI0 T5C4 FMU_CAP1 */ GPIO_TIM5_CH4IN_2 -#define GPIO_TIM5_CH4OUT /* PI0 T5C4 FMU_CAP1 */ GPIO_TIM5_CH4OUT_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 1 +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* PC12 is nARMED * The GPIO will be set as input while not armed HW will have external HW Pull UP. @@ -214,13 +218,12 @@ #if !defined(TRACE_PINS) # 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 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; /* Power supply control and monitoring GPIOs */ @@ -302,7 +305,6 @@ #define INPUT_CAP1_TIMER 5 #define INPUT_CAP1_CHANNEL /* T5C4 */ 4 #define GPIO_INPUT_CAP1 /* PI0 */ GPIO_TIM5_CH4IN -#define BOARD_CAPTURE_GPIO /* PI0 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTI|GPIO_PIN0) /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -391,7 +393,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -427,7 +428,15 @@ GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_PG6, \ - GPIO_nARMED_INIT \ + GPIO_nARMED_INIT, \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \ + PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \ } #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/px4/fmu-v5x/src/can.c b/boards/px4/fmu-v5x/src/can.c index 1a2775d69899..a383a56eddb7 100644 --- a/boards/px4/fmu-v5x/src/can.c +++ b/boards/px4/fmu-v5x/src/can.c @@ -37,7 +37,26 @@ * Board-specific CAN functions. */ -#ifdef CONFIG_CAN +#if !defined(CONFIG_CAN) + +#include + +#include "board_config.h" + + +__EXPORT +uint16_t board_get_can_interfaces(void) +{ + uint16_t enabled_interfaces = 0x3; + + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_CAN2)) { + enabled_interfaces &= ~(1 << 1); + } + + return enabled_interfaces; +} + +#else #include #include diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp index 395684f690a0..1632e5891e0d 100644 --- a/boards/px4/fmu-v5x/src/init.cpp +++ b/boards/px4/fmu-v5x/src/init.cpp @@ -175,12 +175,6 @@ stm32_boardinitialize(void) const uint32_t gpio[] = PX4_GPIO_INIT_LIST; px4_gpio_init(gpio, arraySize(gpio)); - /* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version - - * otherwise we need to move this after board_determine_hw_info()) */ - static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support"); - - stm32_spiinitialize(); - /* configure USB interfaces */ stm32_usbinitialize(); @@ -220,7 +214,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); - board_spi_reset(10, 0xffff); VDD_3V3_SENSORS4_EN(true); VDD_3V3_SPEKTRUM_POWER_EN(true); @@ -237,6 +230,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } + stm32_spiinitialize(); + + board_spi_reset(10, 0xffff); /* configure the DMA allocator */ @@ -244,23 +240,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif /* initial LED state */ drv_led_start(); diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c index f86793a3fddb..91415af45e2a 100644 --- a/boards/px4/fmu-v5x/src/manifest.c +++ b/boards/px4/fmu-v5x/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -73,24 +75,61 @@ static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; // declared in board_common.h static const px4_hw_mft_item_t hw_mft_list_v0500[] = { { + // PX4_MFT_PX4IO .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, }; +static const px4_hw_mft_item_t hw_mft_list_v0550[] = { + { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + static const px4_hw_mft_item_t hw_mft_list_v0510[] = { { + // PX4_MFT_PX4IO .present = 0, .mandatory = 0, .connection = px4_hw_con_unknown, }, { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, @@ -99,28 +138,42 @@ static const px4_hw_mft_item_t hw_mft_list_v0510[] = { static const px4_hw_mft_item_t hw_mft_list_v0509[] = { { + // PX4_MFT_PX4IO .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, { + // PX4_MFT_USB .present = 0, .mandatory = 0, .connection = px4_hw_con_unknown, }, + { + // PX4_MFT_CAN2 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, }; static px4_hw_mft_list_entry_t mft_lists[] = { - {0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, - {0x0001, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, - {0x0100, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, - {0x0900, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, - {0x0901, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, - {0x0a00, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, - {0x0a01, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, +// ver_rev + {V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 0 + {V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 1 + {V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 2 + {V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO, Rev 0 + {V5X50, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 0 + {V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 1 + {V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 2 + {V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB, Rev 0 + {V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 + {V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 2 + {V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q), Rev 0 + {V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1 + {V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2 }; - /************************************************************************************ * Name: board_query_manifest * @@ -152,7 +205,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index 74435c586717..e9383647f061 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -35,28 +35,147 @@ #include #include -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), - }, {GPIO::PortD, GPIO::Pin15}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), - }, {GPIO::PortE, GPIO::Pin7}), -// initSPIBus(SPI::Bus::SPI4, { -// // no devices -// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h -// }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V5X00, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + + initSPIHWVersion(V5X01, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V5X02, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V5X50, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V5X51, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V5X52, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), }), }; -static constexpr bool unused = validateSPIConfig(px4_spi_buses); +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v5x/src/timer_config.cpp b/boards/px4/fmu-v5x/src/timer_config.cpp index 3073f0c22fe5..67adba6a23a0 100644 --- a/boards/px4/fmu-v5x/src/timer_config.cpp +++ b/boards/px4/fmu-v5x/src/timer_config.cpp @@ -72,6 +72,7 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortI, GPIO::Pin0}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board new file mode 100644 index 000000000000..5279a489dc80 --- /dev/null +++ b/boards/px4/fmu-v5x/test.px4board @@ -0,0 +1,14 @@ +CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n +CONFIG_DRIVERS_IRLOCK=n +CONFIG_DRIVERS_PCA9685=n +CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_RPM=n +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_GYRO_FFT=n +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_MICROBENCH=y diff --git a/boards/px4/fmu-v6c/bootloader.px4board b/boards/px4/fmu-v6c/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/px4/fmu-v6c/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board new file mode 100644 index 000000000000..6c491152a0bd --- /dev/null +++ b/boards/px4/fmu-v6c/default.px4board @@ -0,0 +1,91 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=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_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +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_L1=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_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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=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_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=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_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin new file mode 100755 index 000000000000..c0e76324faba Binary files /dev/null and b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin differ diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6c/firmware.prototype b/boards/px4/fmu-v6c/firmware.prototype new file mode 100644 index 000000000000..c1e965a647fc --- /dev/null +++ b/boards/px4/fmu-v6c/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 56, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv6C board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv6C", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/px4/fmu-v6c/init/rc.board_defaults b/boards/px4/fmu-v6c/init/rc.board_defaults new file mode 100644 index 000000000000..4a263a2b734f --- /dev/null +++ b/boards/px4/fmu-v6c/init/rc.board_defaults @@ -0,0 +1,20 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + + +param set-default BAT1_V_DIV 18.1 +param set-default BAT2_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 +param set-default BAT2_A_PER_V 36.367515152 + +# 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 diff --git a/boards/px4/fmu-v6c/init/rc.board_sensors b/boards/px4/fmu-v6c/init/rc.board_sensors new file mode 100644 index 000000000000..d09f1347130d --- /dev/null +++ b/boards/px4/fmu-v6c/init/rc.board_sensors @@ -0,0 +1,21 @@ +#!/bin/sh +# +# PX4 FMUv6C specific board sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal SPI bus BMI055 accel/gyro +bmi055 -A -R 4 -s start +bmi055 -G -R 4 -s start + +# Internal SPI bus ICM42688p +icm42688p -R 6 -s start + +# Internal barometer on I2C4 +ms5611 -I -b 4 -a 0x77 start + +# Internal compass on IMU I2C4 +ist8310 -I -b 4 -a 0xc start + +# External compass on GPS/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 diff --git a/boards/px4/fmu-v6c/nuttx-config/Kconfig b/boards/px4/fmu-v6c/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/px4/fmu-v6c/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/px4/fmu-v6c/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..b1a80c85870a --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig @@ -0,0 +1,96 @@ +# +# 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/px4/fmu-v6c/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H753II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6C.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=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_SDCLONE_DISABLE=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 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h new file mode 100644 index 000000000000..abaaef0a2ab4 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -0,0 +1,505 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6c/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_PX4_FMU_V6C_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V6C_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-v6C 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) * 60 = 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(60) +#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) * 60) +#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 = 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 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 PX4 FMUV6C 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-v6c 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_2 /* PA10 */ +#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_UART5_RX GPIO_UART5_RX_3 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ +// GPIO_UART5_RTS no remap /* PC8 */ +// GPIO_UART5_CTS No remap /* 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_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 /* PB5 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + +/* SPI + * SPI1 is sensors + * SPI2 is FRAM + * + */ + +#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_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 + * + * 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_1 /* PB7 */ + +#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_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 GPIO_I2C4_SCL GPIO_I2C4_SCL_1 /* PD12 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1 /* PD13 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13) + +/* SDMMC2 + * + * VDD 3.3 + * GND + * SDMMC2_CK PD6 + * SDMMC2_CMD PD7 + * SDMMC2_D0 PB14 + * SDMMC2_D1 PB15 + * SDMMC2_D2 PB3 + * 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_2 /* PB3 */ +// 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_PORTA|GPIO_PIN8) /* PA8 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) /* PE13 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) /* PE14 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* PD15 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) /* PA0 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) /* PA1 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); } \ + } 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_V6C_INCLUDE_BOARD_H */ diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..fb688dc51773 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V + +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 BMI055, ICM-42688-P */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 BMI055, ICM-42688-P */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 FRAM */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 FRAM */ + +//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ + +//#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */ +//#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ +//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ + +// 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 + + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +#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 */ + +//#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_1 /* DMA1:81 GPS2 */ +//#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_1 /* DMA1:82 GPS2 */ diff --git a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..886d41030caf --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig @@ -0,0 +1,240 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/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=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CLOCK_MONOTONIC=y +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_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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_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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_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_HSECLOCK=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_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=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_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +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_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=3000 +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_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..c10e790824c0 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * 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 PX4 FMUV6C uses an STM32H753II 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 PX4 FMUV6C 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/px4/fmu-v6c/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..fcc97ec862fd --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * 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 PX4 FMUV6C uses an STM32H753II 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 PX4 FMUV6C 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 = 1920K + + 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/px4/fmu-v6c/src/CMakeLists.txt b/boards/px4/fmu-v6c/src/CMakeLists.txt new file mode 100644 index 000000000000..d7b08bb9c3b1 --- /dev/null +++ b/boards/px4/fmu-v6c/src/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################ +# +# 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 + manifest.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_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h new file mode 100644 index 000000000000..d92fa394a0ec --- /dev/null +++ b/boards/px4/fmu-v6c/src/board_config.h @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * PX4FMU-v6c internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#undef TRACE_PINS + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +#define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* PX4FMU GPIOs ***********************************************************************************/ + + +/* LEDs are driven with push pull Anodes to 3.3V */ + +#define GPIO_nLED_RED /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_nLED_BLUE /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) + +#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 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. */ + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA4 */ GPIO_ADC12_INP18 \ + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY1_CURRENT_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_BATTERY2_VOLTAGE_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_BATTERY1_VOLTAGE_CHANNEL /* PC5 */ ADC1_CH(8) +#define ADC_HW_REV_SENSE_CHANNEL /* PC0 */ ADC3_CH(10) +#define ADC_HW_VER_SENSE_CHANNEL /* PC1 */ ADC3_CH(11) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA2 */ ADC1_CH(14) +#define ADC_SCALED_V5_CHANNEL /* PA4 */ ADC1_CH(18) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL )) + +#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_VERSIONING + +#define GPIO_HW_VER_REV_DRIVE /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) +#define GPIO_HW_REV_SENSE /* PC0 */ GPIO_ADC123_INP10 +#define GPIO_HW_VER_SENSE /* PC1 */ GPIO_ADC123_INP11 +#define HW_INFO_INIT {'V','6','C','x', 'x',0} +#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ +#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets +// Base/FMUM +#define V6C00 HW_VER_REV(0x0,0x0) // FMUV6C, Rev 0 +#define V6C10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 + + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PB9 T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_nPOWER_IN_A /* PA15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15) +#define GPIO_nPOWER_IN_B /* PB12 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN12) +#define GPIO_nPOWER_IN_C /* PE15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ +#define BOARD_NUMBER_BRICKS 2 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ + +#define GPIO_VDD_5V_PERIPH_nEN /* PE2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_VDD_5V_PERIPH_nOC /* PE3 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_nEN /* PC10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN10) +#define GPIO_VDD_5V_HIPOWER_nOC /* PC11 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTC|GPIO_PIN11) +#define GPIO_VDD_3V3_SENSORS_EN /* PB2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2) + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 3 /* Timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* PB0 GPIO_TIM3_CH3OUT_1 */ + +#define GPIO_BUZZER_1 /* PPB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM3_CH3OUT_1 + +/* 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) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 3 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C3 */ 3 +#define GPIO_PWM_IN /* PD14 */ GPIO_TIM4_CH3IN_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_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_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)) +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* FMUv6C never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + + +/* 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_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_nPOWER_IN_C, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SENSORS_EN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_I2C_BUS_MTD 4,5 + + +#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 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/px4/fmu-v6c/src/bootloader_main.c b/boards/px4/fmu-v6c/src/bootloader_main.c new file mode 100644 index 000000000000..c6189fbd042a --- /dev/null +++ b/boards/px4/fmu-v6c/src/bootloader_main.c @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2019, 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); + +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/px4/fmu-v6c/src/can.c b/boards/px4/fmu-v6c/src/can.c new file mode 100644 index 000000000000..3834074f3701 --- /dev/null +++ b/boards/px4/fmu-v6c/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.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 + +#endif /* CONFIG_CAN */ diff --git a/boards/px4/fmu-v6c/src/hw_config.h b/boards/px4/fmu-v6c/src/hw_config.h new file mode 100644 index 000000000000..07bf6f40aaba --- /dev/null +++ b/boards/px4/fmu-v6c/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * 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,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 56 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // RED +#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/px4/fmu-v6c/src/i2c.cpp b/boards/px4/fmu-v6c/src/i2c.cpp new file mode 100644 index 000000000000..61785c89bc11 --- /dev/null +++ b/boards/px4/fmu-v6c/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(4), +}; diff --git a/boards/px4/fmu-v6c/src/init.c b/boards/px4/fmu-v6c/src/init.c new file mode 100644 index 000000000000..5a853b31b163 --- /dev/null +++ b/boards/px4/fmu-v6c/src/init.c @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * 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 + * + * PX4FMU-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 +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * 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 */ + + VDD_5V_PERIPH_EN(false); + board_control_spi_sensors_power(false, 0xffff); + + /* 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); + VDD_5V_PERIPH_EN(true); + +} + +/************************************************************************************ + * 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(io_timer_channel_get_gpio_output(i)); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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) +{ +#if !defined(BOOTLOADER) + + /* Power on Interfaces */ + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + + 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"); + } + + 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_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +# ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +# endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); +#endif + return OK; +} diff --git a/boards/px4/fmu-v6c/src/led.c b/boards/px4/fmu-v6c/src/led.c new file mode 100644 index 000000000000..442e7636f1d0 --- /dev/null +++ b/boards/px4/fmu-v6c/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU 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 + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#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) + 0, // 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/px4/fmu-v6c/src/manifest.c b/boards/px4/fmu-v6c/src/manifest.c new file mode 100644 index 000000000000..04d2386163d0 --- /dev/null +++ b/boards/px4/fmu-v6c/src/manifest.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2018, 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_v0600[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0610[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, + {V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp new file mode 100644 index 000000000000..530c7a3beff2 --- /dev/null +++ b/boards/px4/fmu-v6c/src/mtd.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 +#include +// KiB BS nB +static const px4_mft_device_t spi2 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(4, 0x50) +}; + + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi2, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_entry_t imu_eeprom = { + .device = &i2c4, + .npart = 2, + .partd = { + { + .type = MTD_CALDATA, + .path = "/fs/mtd_caldata", + .nblocks = 248 + }, + { + .type = MTD_ID, + .path = "/fs/mtd_id", + .nblocks = 8 // 256 = 32 * 8 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 2, + .entries = { + &fmum_fram, + &imu_eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/px4/fmu-v6c/src/sdio.c b/boards/px4/fmu-v6c/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/px4/fmu-v6c/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/px4/fmu-v6c/src/spi.cpp b/boards/px4/fmu-v6c/src/spi.cpp new file mode 100644 index 000000000000..9e636e0ac4f9 --- /dev/null +++ b/boards/px4/fmu-v6c/src/spi.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 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 + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V6C00, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), + initSPIHWVersion(V6C10, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6c/src/timer_config.cpp b/boards/px4/fmu-v6c/src/timer_config.cpp new file mode 100644 index 000000000000..e26a8c5d1e2c --- /dev/null +++ b/boards/px4/fmu-v6c/src/timer_config.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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 + * + * TIM1_CH1 T FMU_CH1 + * TIM1_CH2 T FMU_CH2 + * TIM1_CH3 T FMU_CH3 + * TIM1_CH4 T FMU_CH4 + * + * TIM4_CH3 T FMU_CH5 + * TIM4_CH4 T FMU_CH6 + * + * TIM5_CH1 T FMU_CH7 + * TIM5_CH2 T FMU_CH8 + * + * TIM17_CH1 T HEATER > PWM OUT or GPIO + * + * TIM3_CH3 T BUZZER_1 - Driven by other driver + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5), + initIOTimer(Timer::Timer17), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/px4/fmu-v6c/src/usb.c b/boards/px4/fmu-v6c/src/usb.c new file mode 100644 index 000000000000..9d5915c0e6d1 --- /dev/null +++ b/boards/px4/fmu-v6c/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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#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 PX4FMU 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/px4/fmu-v6u/bootloader.cmake b/boards/px4/fmu-v6u/bootloader.cmake deleted file mode 100644 index f399f5f88585..000000000000 --- a/boards/px4/fmu-v6u/bootloader.cmake +++ /dev/null @@ -1,20 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6u - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - - - DRIVERS -# bootloader - - MODULES - - SYSTEMCMDS - - EXAMPLES - - ) diff --git a/boards/px4/fmu-v6u/bootloader.px4board b/boards/px4/fmu-v6u/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/px4/fmu-v6u/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake deleted file mode 100644 index 9f5ddbba6602..000000000000 --- a/boards/px4/fmu-v6u/default.cmake +++ /dev/null @@ -1,132 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6u - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm42605 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board new file mode 100644 index 000000000000..b03e7ec5811d --- /dev/null +++ b/boards/px4/fmu-v6u/default.px4board @@ -0,0 +1,103 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin b/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin new file mode 100755 index 000000000000..f0ac3b98743a Binary files /dev/null and b/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin differ diff --git a/boards/px4/fmu-v6u/init/rc.board_mavlink b/boards/px4/fmu-v6u/init/rc.board_mavlink deleted file mode 100644 index 106d8d207c6e..000000000000 --- a/boards/px4/fmu-v6u/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv6U specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig index 71b63bb5b0e5..17da3161c3c7 100644 --- a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6u/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0036 CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6U.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,8 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y CONFIG_TTY_SIGINT_CHAR=0x03 -CONFIG_TTY_SIGSTP=y +CONFIG_TTY_SIGTSTP=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=300 CONFIG_USBDEV=y diff --git a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig index 9fb11bf7c00b..b1f2c9c2ed8c 100644 --- a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6u/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95751 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0036 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6U.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -71,6 +74,7 @@ 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 @@ -85,7 +89,6 @@ CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -99,16 +102,11 @@ CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -117,16 +115,18 @@ 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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=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_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -140,8 +140,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -193,7 +192,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM12=y CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM4=y diff --git a/boards/px4/fmu-v6u/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6u/nuttx-config/scripts/script.ld index 18b6c69cd8c9..673116d74e0b 100644 --- a/boards/px4/fmu-v6u/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v6u/nuttx-config/scripts/script.ld @@ -109,16 +109,17 @@ MEMORY { - itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K - flash (rx) : ORIGIN = 0x08020000, LENGTH = 1920K - 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 + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + 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) @@ -157,7 +158,7 @@ SECTIONS *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); - } > flash + } > FLASH /* * Init functions (static constructors and the like) @@ -166,17 +167,17 @@ SECTIONS _sinit = ABSOLUTE(.); KEEP(*(.init_array .init_array.*)) _einit = ABSOLUTE(.); - } > flash + } > FLASH .ARM.extab : { *(.ARM.extab*) - } > flash + } > FLASH __exidx_start = ABSOLUTE(.); .ARM.exidx : { *(.ARM.exidx*) - } > flash + } > FLASH __exidx_end = ABSOLUTE(.); _eronly = ABSOLUTE(.); @@ -187,7 +188,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > sram AT > flash + + /* 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(.); @@ -196,7 +202,7 @@ SECTIONS *(COMMON) . = ALIGN(4); _ebss = ABSOLUTE(.); - } > sram + } > AXI_SRAM /* Emit the the D3 power domain section for locating BDMA data */ @@ -205,8 +211,7 @@ SECTIONS *(.sram4) . = ALIGN(4); _sram4_heap_start = ABSOLUTE(.); - } > sram4 - + } > SRAM4 /* Stabs debugging sections. */ .stab 0 : { *(.stab) } diff --git a/boards/px4/fmu-v6u/src/CMakeLists.txt b/boards/px4/fmu-v6u/src/CMakeLists.txt index 2e4206dc4acc..7ff9cdf5f504 100644 --- a/boards/px4/fmu-v6u/src/CMakeLists.txt +++ b/boards/px4/fmu-v6u/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers # sdio bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/px4/fmu-v6u/src/board_config.h b/boards/px4/fmu-v6u/src/board_config.h index 5fd7b35c59f9..890986ea0eca 100644 --- a/boards/px4/fmu-v6u/src/board_config.h +++ b/boards/px4/fmu-v6u/src/board_config.h @@ -164,18 +164,10 @@ #define SYSTEM_ADC_BASE STM32_ADC1_BASE -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* 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_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) @@ -189,20 +181,7 @@ * PWM in future */ #define GPIO_HEATER_OUTPUT /* PA2 T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) - -/* PWM Capture - * - * 1 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PE11 : TIM1_CH2 - */ - -#define GPIO_TIM1_CH2IN /* PE11 T1C2 FMU_CAP1 */ GPIO_TIM1_CH2IN_2 -#define GPIO_TIM1_CH2OUT /* PE11 T1C2 FMU_CAP1 */ GPIO_TIM1_CH2OUT_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 1 +#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. @@ -215,10 +194,8 @@ /* PWM */ -#define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; /* Power supply control and monitoring GPIOs */ @@ -379,7 +356,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/px4/fmu-v6u/src/bootloader_main.c b/boards/px4/fmu-v6u/src/bootloader_main.c index 677e5dec2a75..915b7f3d2997 100644 --- a/boards/px4/fmu-v6u/src/bootloader_main.c +++ b/boards/px4/fmu-v6u/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/px4/fmu-v6u/src/init.c b/boards/px4/fmu-v6u/src/init.c index b835ca2d4ce4..156333190905 100644 --- a/boards/px4/fmu-v6u/src/init.c +++ b/boards/px4/fmu-v6u/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -239,26 +240,12 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } -#if 0 // serial DMA is not yet implemented in NuttX for stm32h7 - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + 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); @@ -279,7 +266,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/px4/fmu-v6u/src/manifest.c b/boards/px4/fmu-v6u/src/manifest.c index 73651c55e245..761b14efa273 100644 --- a/boards/px4/fmu-v6u/src/manifest.c +++ b/boards/px4/fmu-v6u/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -114,7 +116,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v6u/src/timer_config.cpp b/boards/px4/fmu-v6u/src/timer_config.cpp index a3700d5e0300..928f4916f0b8 100644 --- a/boards/px4/fmu-v6u/src/timer_config.cpp +++ b/boards/px4/fmu-v6u/src/timer_config.cpp @@ -73,6 +73,7 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/px4/fmu-v6u/test.cmake b/boards/px4/fmu-v6u/test.cmake deleted file mode 100644 index 1b76b330ffb3..000000000000 --- a/boards/px4/fmu-v6u/test.cmake +++ /dev/null @@ -1,135 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6u - LABEL test - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm42605 - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - #pwm_input - Need to create arch/stm32 arch/stm32h7 - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - fake_gyro - fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v6x/bootloader.cmake b/boards/px4/fmu-v6x/bootloader.cmake deleted file mode 100644 index 33e54346f989..000000000000 --- a/boards/px4/fmu-v6x/bootloader.cmake +++ /dev/null @@ -1,20 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6x - LABEL bootloader - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - - - DRIVERS -# bootloader - - MODULES - - SYSTEMCMDS - - EXAMPLES - - ) diff --git a/boards/px4/fmu-v6x/bootloader.px4board b/boards/px4/fmu-v6x/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/px4/fmu-v6x/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake deleted file mode 100644 index 3fa8ac3295fb..000000000000 --- a/boards/px4/fmu-v6x/default.cmake +++ /dev/null @@ -1,139 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6x - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - UAVCAN_TIMER_OVERRIDE 2 - ETHERNET - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/ads1115 - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/bosch/bmi088 - imu/invensense/icm20602 - imu/invensense/icm20649 - imu/invensense/icm42688p - irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - pca9685 - pca9685_pwm_out - power_monitor/ina226 - #protocol_splitter - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - #uuv_att_control - #uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - netman - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time -# tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - fake_gps - #fake_gyro - #fake_magnetometer - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board new file mode 100644 index 000000000000..b9de1c2fb199 --- /dev/null +++ b/boards/px4/fmu-v6x/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=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_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=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_L1=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_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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=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_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=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/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin new file mode 100755 index 000000000000..0d95b43f8afa Binary files /dev/null and b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin differ diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..36128d00f426 Binary files /dev/null and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index 9c542a77300e..a7d844a36632 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -3,4 +3,17 @@ # 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 1 + safety_button start diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink deleted file mode 100644 index 5146dc069c96..000000000000 --- a/boards/px4/fmu-v6x/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv6X specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 4285cab6790c..4facbe4c0539 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -2,29 +2,83 @@ # # PX4 FMUv6X specific board sensors init #------------------------------------------------------------------------------ -board_adc start +set HAVE_PM2 yes -# Start Digital power monitors -ina226 -X -b 1 -t 1 -k start -ina226 -X -b 2 -t 2 -k start +if ver hwtypecmp V6X50 V6X51 V6X53 V6X54 +then + set HAVE_PM2 no +fi +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 + + if [ $HAVE_PM2 = yes ] + then + ina226 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA228 1 +then + # Start Digital power monitors + ina228 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina228 -X -b 2 -t 2 -k start + fi +fi + +if param compare SENS_EN_INA238 1 +then + # Start Digital power monitors + ina238 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] + then + ina238 -X -b 2 -t 2 -k start + fi +fi +if ver hwtypecmp V6X04 V6X14 V6X54 +then + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start +else # Internal SPI BMI088 -bmi088 -A -R 4 -s start -bmi088 -G -R 4 -s start + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start +fi # Internal SPI bus ICM42688p icm42688p -R 6 -s start -# Internal SPI bus ICM-20649 (hard-mounted) -icm20649 -R 14 -s start +if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54 +then + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start +else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start +fi # Internal magnetometer on I2c bmm150 -I 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 bmp388 -I -a 0x77 start -if ver hwtypecmp V6X00 +if ver hwtypecmp V6X00 V6X10 then bmp388 -I start else @@ -33,3 +87,5 @@ fi # Baro on I2C3 ms5611 -X start + +unset HAVE_PM2 \ No newline at end of file diff --git a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig index 51719a40ebf5..40e04d91789d 100644 --- a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig @@ -11,7 +11,7 @@ # CONFIG_STM32H7_SYSCFG is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6x/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -29,6 +29,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0035 CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6X.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -51,10 +52,8 @@ CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_LIB_BOARDCTL=y -CONFIG_MAX_TASKS=8 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y -CONFIG_NFILE_DESCRIPTORS=5 CONFIG_PREALLOC_TIMERS=50 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=245760 @@ -73,19 +72,22 @@ 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_USART3=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_SIGSTP=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_TXBUFSIZE=300 +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 diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h index 768597447439..1f45efc569d1 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board_dma_map.h @@ -33,19 +33,55 @@ #pragma once -// DMAMUX1 -#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ -#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V -#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ -#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 ICM-20649 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 ICM-20649 */ -#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* DMA1:71 */ -#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* DMA1:72 */ +#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 */ -// DMAMUX2 -#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_1 /* DMA2:61 */ -#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_1 /* DMA2:62 */ +//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ -#define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* BDMA:11 */ -#define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* BDMA:12 */ +//#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */ +//#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ +//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ + +// 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 + +#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_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +#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 */ + +//#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_1 /* DMA1:81 GPS2 */ +//#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_1 /* DMA1:82 GPS2 */ + +// 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/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index a6a3f7c6a4a2..274c7617a373 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6x/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -37,6 +38,7 @@ 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_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95751 @@ -44,6 +46,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0035 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6X.x" CONFIG_CDCACM_RXBUFSIZE=600 @@ -73,6 +76,7 @@ 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 @@ -90,7 +94,6 @@ CONFIG_IPCFG_PATH="/fs/mtd_net" CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -118,26 +121,22 @@ CONFIG_NETUTILS_TELNETD=y CONFIG_NET_ARP_IPIN=y CONFIG_NET_ARP_SEND=y CONFIG_NET_BROADCAST=y +CONFIG_NET_ETH_PKTSIZE=1518 CONFIG_NET_ICMP=y CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=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_UDP=y CONFIG_NET_UDP_CHECKSUMS=y CONFIG_NET_UDP_WRITE_BUFFERS=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -148,13 +147,13 @@ CONFIG_NSH_STRERROR=y CONFIG_NSH_TELNET=y CONFIG_NSH_TELNET_LOGIN=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_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y @@ -173,8 +172,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -232,7 +230,6 @@ CONFIG_STM32H7_SPI6=y CONFIG_STM32H7_SPI6_DMA=y CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM12=y CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM4=y @@ -259,11 +256,16 @@ CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART5_IFLOWCONTROL=y CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +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_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 CONFIG_UART8_TXBUFSIZE=1500 @@ -277,8 +279,10 @@ CONFIG_USART2_RXBUFSIZE=600 CONFIG_USART2_TXBUFSIZE=3000 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_TXBUFSIZE=1500 diff --git a/boards/px4/fmu-v6x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6x/nuttx-config/scripts/script.ld index 66bd6130c29a..b6b341d4e84b 100644 --- a/boards/px4/fmu-v6x/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v6x/nuttx-config/scripts/script.ld @@ -109,16 +109,17 @@ MEMORY { - itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K - flash (rx) : ORIGIN = 0x08020000, LENGTH = 1920K - 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 + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + 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) @@ -157,7 +158,7 @@ SECTIONS *(.gnu.linkonce.r.*) _etext = ABSOLUTE(.); - } > flash + } > FLASH /* * Init functions (static constructors and the like) @@ -166,17 +167,17 @@ SECTIONS _sinit = ABSOLUTE(.); KEEP(*(.init_array .init_array.*)) _einit = ABSOLUTE(.); - } > flash + } > FLASH .ARM.extab : { *(.ARM.extab*) - } > flash + } > FLASH __exidx_start = ABSOLUTE(.); .ARM.exidx : { *(.ARM.exidx*) - } > flash + } > FLASH __exidx_end = ABSOLUTE(.); _eronly = ABSOLUTE(.); @@ -187,7 +188,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > sram AT > flash + + /* 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(.); @@ -196,7 +202,7 @@ SECTIONS *(COMMON) . = ALIGN(4); _ebss = ABSOLUTE(.); - } > sram + } > AXI_SRAM /* Emit the the D3 power domain section for locating BDMA data */ @@ -205,7 +211,7 @@ SECTIONS *(.sram4) . = ALIGN(4); _sram4_heap_start = ABSOLUTE(.); - } > sram4 + } > SRAM4 /* Stabs debugging sections. */ .stab 0 : { *(.stab) } diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index 2e4206dc4acc..7ff9cdf5f504 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -41,7 +41,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") nuttx_drivers # sdio bootloader ) - target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) else() add_library(drivers_board diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index d959e958cffc..f72f2c134c4f 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016, 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -202,18 +202,10 @@ #define SYSTEM_ADC_BASE STM32_ADC1_BASE -/* Define Battery 1 Voltage Divider and A per V - */ - -#define BOARD_BATTERY1_V_DIV (18.1f) /* measured with the provided PM board */ -#define BOARD_BATTERY1_A_PER_V (36.367515152f) - /* 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_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) @@ -223,24 +215,27 @@ #define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ #define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 // Rev 0 and Rev 3,4 Sensor sets +// Base/FMUM +#define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 +#define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 +#define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 +#define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 +#define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 +#define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 +#define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 +#define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 +#define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 +#define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 +#define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 + +#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) - -/* PWM Capture - * - * 1 PWM Capture inputs are configured. - * - * Pins: - * - * FMU_CAP1 : PE11 : TIM1_CH2 - */ - -#define GPIO_TIM1_CH2IN /* PE11 T1C2 FMU_CAP1 */ GPIO_TIM1_CH2IN_2 -#define GPIO_TIM1_CH2OUT /* PE11 T1C2 FMU_CAP1 */ GPIO_TIM1_CH2OUT_2 - -#define DIRECT_PWM_CAPTURE_CHANNELS 1 +#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. @@ -250,15 +245,14 @@ #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 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 +#define DIRECT_PWM_OUTPUT_CHANNELS 9 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; /* Power supply control and monitoring GPIOs */ @@ -426,7 +420,6 @@ #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 diff --git a/boards/px4/fmu-v6x/src/bootloader_main.c b/boards/px4/fmu-v6x/src/bootloader_main.c index 677e5dec2a75..915b7f3d2997 100644 --- a/boards/px4/fmu-v6x/src/bootloader_main.c +++ b/boards/px4/fmu-v6x/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) void board_late_initialize(void) { - px4_platform_console_init(); sercon_main(0, NULL); } diff --git a/boards/px4/fmu-v6x/src/can.c b/boards/px4/fmu-v6x/src/can.c index 3834074f3701..e11790a65963 100644 --- a/boards/px4/fmu-v6x/src/can.c +++ b/boards/px4/fmu-v6x/src/can.c @@ -37,7 +37,26 @@ * Board-specific CAN functions. */ -#ifdef CONFIG_CAN +#if !defined(CONFIG_CAN) + +#include + +#include "board_config.h" + + +__EXPORT +uint16_t board_get_can_interfaces(void) +{ + uint16_t enabled_interfaces = 0x3; + + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_CAN2)) { + enabled_interfaces &= ~(1 << 1); + } + + return enabled_interfaces; +} + +#else #include #include @@ -52,8 +71,6 @@ #include "stm32_can.h" #include "board_config.h" -#ifdef CONFIG_CAN - /************************************************************************************ * Pre-processor Definitions ************************************************************************************/ @@ -122,7 +139,4 @@ int can_devinit(void) return OK; } - -#endif - #endif /* CONFIG_CAN */ diff --git a/boards/px4/fmu-v6x/src/init.c b/boards/px4/fmu-v6x/src/init.c index 5436d1bf389b..a96a406496ae 100644 --- a/boards/px4/fmu-v6x/src/init.c +++ b/boards/px4/fmu-v6x/src/init.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * 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 @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -172,12 +173,6 @@ stm32_boardinitialize(void) const uint32_t gpio[] = PX4_GPIO_INIT_LIST; px4_gpio_init(gpio, arraySize(gpio)); - /* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version - - * otherwise we need to move this after board_determine_hw_info()) */ - _Static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support"); - - stm32_spiinitialize(); - /* configure USB interfaces */ stm32_usbinitialize(); @@ -211,14 +206,12 @@ stm32_boardinitialize(void) * ****************************************************************************/ - __EXPORT int board_app_initialize(uintptr_t arg) { /* Power on Interfaces */ VDD_3V3_SD_CARD_EN(true); VDD_5V_PERIPH_EN(true); VDD_5V_HIPOWER_EN(true); - board_spi_reset(10, 0xffff); VDD_3V3_SENSORS4_EN(true); VDD_3V3_SPEKTRUM_POWER_EN(true); @@ -235,32 +228,22 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } + 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 0 // serial DMA is not yet implemented in NuttX for stm32h7 - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + 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); @@ -281,7 +264,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif /* CONFIG_MMCSD */ diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index 197b65412aa1..c5a3da2831c0 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "systemlib/px4_macros.h" @@ -73,16 +75,81 @@ static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; // declared in board_common.h static const px4_hw_mft_item_t hw_mft_list_v0600[] = { { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0610[] = { + { + // PX4_MFT_PX4IO + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, }; +static const px4_hw_mft_item_t hw_mft_list_v0650[] = { + { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_unknown, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + + static px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev - {0x0000, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {0x0001, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 + {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, + {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 + {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 + {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini + {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini + {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 + {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 + {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO + {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 }; /************************************************************************************ @@ -116,7 +183,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) } if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %4x is not supported!\n", ver_rev); + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); } } diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index 5ec14e7326c9..e25327001d81 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2020, 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 @@ -35,28 +35,145 @@ #include #include -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - }, {GPIO::PortE, GPIO::Pin7}), -// initSPIBus(SPI::Bus::SPI4, { -// // no devices -// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h -// }, {GPIO::PortG, GPIO::Pin8}), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V6X00, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + + initSPIHWVersion(V6X03, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V6X50, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X04, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X53, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X54, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), }), }; -static constexpr bool unused = validateSPIConfig(px4_spi_buses); +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6x/src/timer_config.cpp b/boards/px4/fmu-v6x/src/timer_config.cpp index a3700d5e0300..928f4916f0b8 100644 --- a/boards/px4/fmu-v6x/src/timer_config.cpp +++ b/boards/px4/fmu-v6x/src/timer_config.cpp @@ -73,6 +73,7 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + initIOTimerChannelCapture(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), }; constexpr io_timers_channel_mapping_t io_timers_channel_mapping = diff --git a/boards/px4/io-v2/default.cmake b/boards/px4/io-v2/default.cmake deleted file mode 100644 index 21956f2e9b72..000000000000 --- a/boards/px4/io-v2/default.cmake +++ /dev/null @@ -1,12 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL io-v2 - TOOLCHAIN arm-none-eabi - CONSTRAINED_FLASH - ARCHITECTURE cortex-m3 - DRIVERS - MODULES - px4iofirmware - ) diff --git a/boards/px4/io-v2/default.px4board b/boards/px4/io-v2/default.px4board new file mode 100644 index 000000000000..9e69a1e7460d --- /dev/null +++ b/boards/px4/io-v2/default.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m3" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_MODULES_PX4IOFIRMWARE=y diff --git a/boards/px4/io-v2/bootloader/px4_io-v2_bootloader.bin b/boards/px4/io-v2/extras/px4_io-v2_bootloader.bin similarity index 100% rename from boards/px4/io-v2/bootloader/px4_io-v2_bootloader.bin rename to boards/px4/io-v2/extras/px4_io-v2_bootloader.bin diff --git a/boards/px4/io-v2/nuttx-config/nsh/defconfig b/boards/px4/io-v2/nuttx-config/nsh/defconfig index 97a82523b6a4..1444f21613bb 100644 --- a/boards/px4/io-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/io-v2/nuttx-config/nsh/defconfig @@ -8,13 +8,14 @@ # CONFIG_DEV_NULL is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/io-v2/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_STM32F100C8=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_C99_BOOL8=y CONFIG_DEBUG_FULLOPT=y @@ -27,12 +28,10 @@ CONFIG_FDCLONE_DISABLE=y CONFIG_FDCLONE_STDIO=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_IDLETHREAD_STACKSIZE=280 -CONFIG_MAX_TASKS=2 +CONFIG_IDLETHREAD_STACKSIZE=316 CONFIG_MM_FILL_ALLOCATIONS=y CONFIG_MM_SMALL=y CONFIG_NAME_MAX=12 -CONFIG_NFILE_DESCRIPTORS=3 CONFIG_PREALLOC_TIMERS=0 CONFIG_RAM_SIZE=8192 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/io-v2/src/board_config.h b/boards/px4/io-v2/src/board_config.h index 1a83e884451b..e934397996db 100644 --- a/boards/px4/io-v2/src/board_config.h +++ b/boards/px4/io-v2/src/board_config.h @@ -47,8 +47,6 @@ #include #include -#include - /****************************************************************************** * Definitions ******************************************************************************/ @@ -72,12 +70,10 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) -#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) - -#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED_BLUE (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) // IO-LED_BLUE or IMU_TEMPERATURE_CONTROL on Pixhawk 2.1 +#define GPIO_LED_AMBER (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY +#define GPIO_LED_GREEN (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING #define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) #define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) @@ -90,12 +86,24 @@ * PC14 3.3v * PC15 GND */ - #define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14) #define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15) # define SENSE_PH1 0b10 /* Floating pulled as set */ # define SENSE_PH2 0b01 /* Driven as tied */ +#define SENSE_PIXHAWK2() (((stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN)) == SENSE_PH2) ? 1 : 0) + +#define LED_BLUE(on_true) (SENSE_PIXHAWK2() ? (void)0 : stm32_gpiowrite(GPIO_LED_BLUE, !(on_true))) +#define LED_AMBER(on_true) stm32_gpiowrite(GPIO_LED_AMBER, !(on_true)) +#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true)) +#define LED_GREEN(on_true) stm32_gpiowrite(GPIO_LED_GREEN, (on_true)) + + +/* HEATER */ +#define GPIO_HEATER_OUTPUT /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +#define HEATER_OUTPUT_EN(on_true) (SENSE_PIXHAWK2() ? stm32_gpiowrite(GPIO_HEATER_OUTPUT, !(on_true)) : (void)0) + + #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) /* Safety switch button *******************************************************/ @@ -135,6 +143,7 @@ #define GPIO_PWM8 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) #define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define BOARD_HAS_NO_CAPTURE /* SBUS pins *************************************************************/ @@ -151,18 +160,6 @@ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 PA8 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8) -/* LED definitions ******************************************************************/ -/* PX4 has two LEDs that we will encode as: */ - -#define LED_STARTED 0 /* LED? */ -#define LED_HEAPALLOCATE 1 /* LED? */ -#define LED_IRQSENABLED 2 /* LED? + LED? */ -#define LED_STACKCREATED 3 /* LED? */ -#define LED_INIRQ 4 /* LED? + LED? */ -#define LED_SIGNAL 5 /* LED? + LED? */ -#define LED_ASSERTION 6 /* LED? + LED? + LED? */ -#define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ - #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DISABLE_I2C_SPI +#include diff --git a/boards/px4/io-v2/src/init.c b/boards/px4/io-v2/src/init.c index 10c081d08b74..404a3620f79f 100644 --- a/boards/px4/io-v2/src/init.c +++ b/boards/px4/io-v2/src/init.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4iov2_init.c + * @file init.c * * PX4FMU-specific early startup code. This file implements the * stm32_boardinitialize() function that is called during cpu startup. @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -83,39 +84,33 @@ __EXPORT void stm32_boardinitialize(void) { - /* configure GPIOs */ /* Set up for sensing HW */ - stm32_configgpio(GPIO_SENSE_PC14_DN); stm32_configgpio(GPIO_SENSE_PC15_UP); - /* LEDS - default to off */ - stm32_configgpio(GPIO_LED1); - stm32_configgpio(GPIO_LED2); - stm32_configgpio(GPIO_LED3); - stm32_configgpio(GPIO_LED4); - - /* PixHawk 1: - * PC14 Floating - * PC15 Floating - * - * PixHawk 2: - * PC14 3.3v - * PC15 GND - */ - - uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN); - - if (sense == SENSE_PH2) { - stm32_configgpio(GPIO_HEATER_OFF); + /* some boards such as Pixhawk 2.1 made + the unfortunate choice to combine the blue led channel with + the IMU heater. We need a software hack to fix the hardware hack + by allowing to disable the LED / heater. + */ + if (SENSE_PIXHAWK2()) { + stm32_configgpio(GPIO_HEATER_OUTPUT); + + } else { + stm32_configgpio(GPIO_LED_BLUE); } stm32_configgpio(GPIO_PC14); stm32_configgpio(GPIO_PC15); + /* LEDS - default to off */ + stm32_configgpio(GPIO_LED_AMBER); + stm32_configgpio(GPIO_LED_SAFETY); + stm32_configgpio(GPIO_LED_GREEN); + stm32_configgpio(GPIO_BTN_SAFETY); /* spektrum power enable is active high - enable it by default */ @@ -162,4 +157,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_gpiowrite(GPIO_PWM8, true); stm32_configgpio(GPIO_PWM8); + + /* disable heater */ + HEATER_OUTPUT_EN(false); } diff --git a/boards/px4/io-v2/src/timer_config.cpp b/boards/px4/io-v2/src/timer_config.cpp index f84e69921cb0..bb7dfa86fe53 100644 --- a/boards/px4/io-v2/src/timer_config.cpp +++ b/boards/px4/io-v2/src/timer_config.cpp @@ -35,8 +35,8 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer2), - initIOTimer(Timer::Timer3), initIOTimer(Timer::Timer4), + initIOTimer(Timer::Timer3), }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake deleted file mode 100644 index b029484c3422..000000000000 --- a/boards/px4/raspberrypi/default.cmake +++ /dev/null @@ -1,105 +0,0 @@ -add_definitions( - -D__PX4_LINUX -) - -px4_add_board( - VENDOR px4 - MODEL raspberrypi - LABEL default - PLATFORM posix - ARCHITECTURE cortex-a53 - ROMFSROOT px4fmu_common - TOOLCHAIN arm-linux-gnueabihf - TESTING - DRIVERS - adc/ads1115 - #barometer # all available barometer drivers - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - imu/invensense/mpu9250 - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - pca9685_pwm_out - pwm_out_sim - rc_input - rpi_rc_in - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - #simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - dyn - esc_calib - led_control - mixer - motor_ramp - motor_test - param - perf - pwm - sd_bench - #serial_test - system_time - shutdown - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board new file mode 100644 index 000000000000..5cd14674eb60 --- /dev/null +++ b/boards/px4/raspberrypi/default.px4board @@ -0,0 +1,85 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_LINUX=y +CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf" +CONFIG_BOARD_ARCHITECTURE="cortex-a53" +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_RPI_RC_IN=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_FIXEDWING_CONTROL=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y +CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/px4/raspberrypi/src/board_config.h b/boards/px4/raspberrypi/src/board_config.h index 8b8921177f66..14653d150e4c 100644 --- a/boards/px4/raspberrypi/src/board_config.h +++ b/boards/px4/raspberrypi/src/board_config.h @@ -42,11 +42,16 @@ #define BOARD_OVERRIDE_UUID "RPIID00000000000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_RPI -/* - * I2C busses - */ + +// I2C +#define CONFIG_I2C 1 #define PX4_NUMBER_I2C_BUSES 2 + +// SPI +#define CONFIG_SPI 1 + + #define ADC_BATTERY_VOLTAGE_CHANNEL 0 #define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2 diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake deleted file mode 100644 index 876ad06dcba5..000000000000 --- a/boards/px4/sitl/ctrlalloc.cmake +++ /dev/null @@ -1,122 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL ctrlalloc - EMBEDDED_METADATA parameters - TESTING - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - angular_velocity_controller - attitude_estimator_q - camera_feedback - commander - control_allocator - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - failure - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger disable CACHE STRING "debugger for sitl") -set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") - -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake deleted file mode 100644 index d2221a0cdfb1..000000000000 --- a/boards/px4/sitl/default.cmake +++ /dev/null @@ -1,122 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL default - EMBEDDED_METADATA parameters - TESTING - ETHERNET - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - failure - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger disable CACHE STRING "debugger for sitl") -set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") - -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board new file mode 100644 index 000000000000..fc401133bcfb --- /dev/null +++ b/boards/px4/sitl/default.px4board @@ -0,0 +1,76 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_TESTING=y +CONFIG_BOARD_ETHERNET=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RPM_RPM_SIMULATOR=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_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_L1=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_RC_UPDATE=y +CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_SIMULATOR=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y +CONFIG_EXAMPLES_FIXEDWING_CONTROL=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y +CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake deleted file mode 100644 index 83e40ed7b6f8..000000000000 --- a/boards/px4/sitl/nolockstep.cmake +++ /dev/null @@ -1,105 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL nolockstep - EMBEDDED_METADATA parameters - TESTING - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - failure - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -message(STATUS "Building without lockstep") -set(ENABLE_LOCKSTEP_SCHEDULER no) - diff --git a/boards/px4/sitl/nolockstep.px4board b/boards/px4/sitl/nolockstep.px4board new file mode 100644 index 000000000000..61671c9a7791 --- /dev/null +++ b/boards/px4/sitl/nolockstep.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_NOLOCKSTEP=y diff --git a/boards/px4/sitl/replay.cmake b/boards/px4/sitl/replay.cmake deleted file mode 100644 index fca852c46c6e..000000000000 --- a/boards/px4/sitl/replay.cmake +++ /dev/null @@ -1,24 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL replay - MODULES - ekf2 - logger - replay - SYSTEMCMDS - param - perf - shutdown - topic_listener - ver - work_queue - ) - -message(STATUS "Building with uorb publisher rules support") -add_definitions(-DORB_USE_PUBLISHER_RULES) - -set(ENABLE_LOCKSTEP_SCHEDULER yes) diff --git a/boards/px4/sitl/replay.px4board b/boards/px4/sitl/replay.px4board new file mode 100644 index 000000000000..aab3a2793d60 --- /dev/null +++ b/boards/px4/sitl/replay.px4board @@ -0,0 +1,2 @@ +CONFIG_BOARD_COMPILE_DEFINITIONS="-DORB_USE_PUBLISHER_RULES" +CONFIG_BOARD_LOCKSTEP=y diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake deleted file mode 100644 index 63eb8e398b44..000000000000 --- a/boards/px4/sitl/rtps.cmake +++ /dev/null @@ -1,119 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL rtps - EMBEDDED_METADATA parameters - TESTING - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - #protocol_splitter - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger disable CACHE STRING "debugger for sitl") -set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") - -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) - - message(STATUS "Building without lockstep for replay") - set(ENABLE_LOCKSTEP_SCHEDULER no) -else() - set(ENABLE_LOCKSTEP_SCHEDULER yes) -endif() diff --git a/boards/px4/sitl/rtps.px4board b/boards/px4/sitl/rtps.px4board new file mode 100644 index 000000000000..c7f399bf6a9a --- /dev/null +++ b/boards/px4/sitl/rtps.px4board @@ -0,0 +1,2 @@ +CONFIG_MODULES_MICRORTPS_BRIDGE=y +CONFIG_MODULES_MICRODDS_CLIENT=y diff --git a/boards/px4/sitl/sitl.cmake b/boards/px4/sitl/sitl.cmake new file mode 100644 index 000000000000..c62e5345c039 --- /dev/null +++ b/boards/px4/sitl/sitl.cmake @@ -0,0 +1,20 @@ +set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") +set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger disable CACHE STRING "debugger for sitl") +set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") + +# If the environment variable 'replay' is defined, we are building with replay +# support. In this case, we enable the orb publisher rules. +set(REPLAY_FILE "$ENV{replay}") +if(REPLAY_FILE) + message(STATUS "Building with uorb publisher rules support") + add_definitions(-DORB_USE_PUBLISHER_RULES) + + message(STATUS "Building without lockstep for replay") + set(ENABLE_LOCKSTEP_SCHEDULER no) +elseif(CMAKE_BUILD_TYPE STREQUAL FuzzTesting) + set(ENABLE_LOCKSTEP_SCHEDULER no) +else() + set(ENABLE_LOCKSTEP_SCHEDULER yes) +endif() diff --git a/boards/px4/sitl/src/CMakeLists.txt b/boards/px4/sitl/src/CMakeLists.txt index 0286396097de..294f9868a4bd 100644 --- a/boards/px4/sitl/src/CMakeLists.txt +++ b/boards/px4/sitl/src/CMakeLists.txt @@ -33,7 +33,5 @@ add_library(drivers_board board_shutdown.cpp - i2c.cpp sitl_led.c - spi.cpp ) diff --git a/boards/px4/sitl/src/board_config.h b/boards/px4/sitl/src/board_config.h index 09294c523288..e27c3ee04fe4 100644 --- a/boards/px4/sitl/src/board_config.h +++ b/boards/px4/sitl/src/board_config.h @@ -42,11 +42,7 @@ #define BOARD_OVERRIDE_UUID "SIMULATIONID0000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_SITL -#define BOARD_BATTERY1_V_DIV (10.177939394f) -#define BOARD_BATTERY1_A_PER_V (15.391030303f) - #define BOARD_HAS_POWER_CONTROL 1 -#define CONFIG_BOARDCTL_POWEROFF 1 #define PX4_NUMBER_I2C_BUSES 1 diff --git a/boards/px4/sitl/src/board_shutdown.cpp b/boards/px4/sitl/src/board_shutdown.cpp index 6ed9329c844a..464fdc83c19d 100644 --- a/boards/px4/sitl/src/board_shutdown.cpp +++ b/boards/px4/sitl/src/board_shutdown.cpp @@ -46,9 +46,7 @@ int board_register_power_state_notification_cb(power_button_state_notification_t { return 0; } -#endif // BOARD_HAS_POWER_CONTROL -#if defined(CONFIG_BOARDCTL_POWEROFF) int board_power_off(int status) { printf("Exiting NOW.\n"); @@ -56,4 +54,4 @@ int board_power_off(int status) system_exit(0); return 0; } -#endif // CONFIG_BOARDCTL_POWEROFF +#endif // BOARD_HAS_POWER_CONTROL diff --git a/boards/px4/sitl/src/i2c.cpp b/boards/px4/sitl/src/i2c.cpp deleted file mode 100644 index bb1b3e5afd5f..000000000000 --- a/boards/px4/sitl/src/i2c.cpp +++ /dev/null @@ -1,38 +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 - -constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { -}; - diff --git a/boards/px4/sitl/src/spi.cpp b/boards/px4/sitl/src/spi.cpp deleted file mode 100644 index ce3841e169a2..000000000000 --- a/boards/px4/sitl/src/spi.cpp +++ /dev/null @@ -1,38 +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 -#include - -constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { -}; diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake deleted file mode 100644 index c228cba95cc4..000000000000 --- a/boards/px4/sitl/test.cmake +++ /dev/null @@ -1,117 +0,0 @@ - -px4_add_board( - PLATFORM posix - VENDOR px4 - MODEL sitl - ROMFSROOT px4fmu_common - LABEL test - EMBEDDED_METADATA parameters - TESTING - DRIVERS - #barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #distance_sensor # all available distance sensor drivers - distance_sensor/lightware_laser_serial - gps - #imu # all available imu drivers - #magnetometer # all available magnetometer drivers - pwm_out_sim - rpm/rpm_simulator - #telemetry # all available telemetry drivers - tone_alarm - #uavcan - MODULES - airship_att_control - airspeed_selector - attitude_estimator_q - camera_feedback - commander - control_allocator - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - replay - rover_pos_control - sensors - #sih - simulator - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - #dumpfile - dyn - esc_calib - led_control - #mft - mixer - motor_ramp - motor_test - #mtd - #nshterm - param - perf - pwm - sd_bench - shutdown - system_time - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) - -set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl") -set_property(CACHE config_sitl_viewer PROPERTY STRINGS "jmavsim;none") - -set(config_sitl_debugger disable CACHE STRING "debugger for sitl") -set_property(CACHE config_sitl_debugger PROPERTY STRINGS "disable;gdb;lldb") - -# If the environment variable 'replay' is defined, we are building with replay -# support. In this case, we enable the orb publisher rules. -set(REPLAY_FILE "$ENV{replay}") -if(REPLAY_FILE) - message(STATUS "Building with uorb publisher rules support") - add_definitions(-DORB_USE_PUBLISHER_RULES) -endif() - -message(STATUS "Building without lockstep for test") -set(ENABLE_LOCKSTEP_SCHEDULER no) diff --git a/boards/px4/sitl/test.px4board b/boards/px4/sitl/test.px4board new file mode 100644 index 000000000000..fcaf472f793e --- /dev/null +++ b/boards/px4/sitl/test.px4board @@ -0,0 +1,2 @@ +CONFIG_BOARD_NOLOCKSTEP=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board new file mode 100644 index 000000000000..79b51f5e26e0 --- /dev/null +++ b/boards/raspberrypi/pico/default.px4board @@ -0,0 +1,53 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m0plus" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=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_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_TOP=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/raspberrypi/pico/firmware.prototype b/boards/raspberrypi/pico/firmware.prototype new file mode 100644 index 000000000000..1fcf0a363b9b --- /dev/null +++ b/boards/raspberrypi/pico/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 42, + "magic": "RASPBERRYPIPICO", + "description": "Firmware for the raspberry pi Pico board", + "image": "", + "build_time": 0, + "summary": "RaspberrypiPico", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1032192, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/raspberrypi/pico/init/rc.board_defaults b/boards/raspberrypi/pico/init/rc.board_defaults new file mode 100644 index 000000000000..ec0c72060898 --- /dev/null +++ b/boards/raspberrypi/pico/init/rc.board_defaults @@ -0,0 +1,21 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 13.653333333 +param set-default BAT1_A_PER_V 36.367515152 + +# system_power unavailable +param set-default CBRK_SUPPLY_CHK 894281 + +# Disable safety switch by default +param set-default CBRK_IO_SAFETY 22027 + +# use the Q attitude estimator, it works w/o mag or GPS. +# param set-default SYS_MC_EST_GROUP 3 +# param set-default ATT_ACC_COMP 0 +# param set-default ATT_W_ACC 0.4000 +# param set-default ATT_W_GYRO_BIAS 0.0000 + +# param set-default SYS_HAS_MAG 0 diff --git a/boards/cuav/nora/init/rc.board_mavlink b/boards/raspberrypi/pico/init/rc.board_mavlink similarity index 100% rename from boards/cuav/nora/init/rc.board_mavlink rename to boards/raspberrypi/pico/init/rc.board_mavlink diff --git a/boards/raspberrypi/pico/init/rc.board_sensors b/boards/raspberrypi/pico/init/rc.board_sensors new file mode 100644 index 000000000000..2246747096d2 --- /dev/null +++ b/boards/raspberrypi/pico/init/rc.board_sensors @@ -0,0 +1,12 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# try starting an mpu9250 IMU on external SPI +mpu9250 start -S + +# try starting a bmp280 barometer on external SPI +bmp280 start -S diff --git a/boards/raspberrypi/pico/nuttx-config/Kconfig b/boards/raspberrypi/pico/nuttx-config/Kconfig new file mode 100644 index 000000000000..f70b26deecc7 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/Kconfig @@ -0,0 +1,25 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config RP2040_FLASH_BOOT + bool "flash boot" + default y + ---help--- + If y, the built binary can be used for flash boot. + If not, the binary is for SRAM boot. + +config RP2040_FLASH_CHIP + string "flash chip name" + default "w25q080" + ---help--- + Name of NOR flash device connected to RP2040 SoC. + (Used to choose the secondary boot loader.) + Basically this option should not be changed. + +config RP2040_UF2_BINARY + bool "uf2 binary format" + default y + ---help--- + Create nuttx.uf2 binary format used on RP2040 based arch. diff --git a/boards/raspberrypi/pico/nuttx-config/include/board.h b/boards/raspberrypi/pico/nuttx-config/include/board.h new file mode 100644 index 000000000000..fe16b07ec846 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/include/board.h @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +/* Clocking *****************************************************************/ + +#define MHZ 1000000 + +#define BOARD_XOSC_FREQ (12 * MHZ) +#define BOARD_PLL_SYS_FREQ (125 * MHZ) +#define BOARD_PLL_USB_FREQ (48 * MHZ) + +#define BOARD_REF_FREQ (12 * MHZ) +#define BOARD_SYS_FREQ (125 * MHZ) +#define BOARD_PERI_FREQ (125 * MHZ) +#define BOARD_USB_FREQ (48 * MHZ) +#define BOARD_ADC_FREQ (48 * MHZ) +#define BOARD_RTC_FREQ 46875 + +#define BOARD_UART_BASEFREQ BOARD_PERI_FREQ + +#define BOARD_TICK_CLOCK (1 * MHZ) + +/* If CONFIG_ARCH_LEDs is defined, then NuttX will control the 2 LEDs on board the + * omnibusf4sd. The following definitions describe how NuttX controls the LEDs: + */ + +// #define LED_STARTED 0 /* LED1 */ +// #define LED_HEAPALLOCATE 1 /* LED2 */ +// #define LED_IRQSENABLED 2 /* LED1 */ +// #define LED_STACKCREATED 3 /* LED1 + LED2 */ +// #define LED_INIRQ 4 /* LED1 */ +// #define LED_SIGNAL 5 /* LED2 */ +// #define LED_ASSERTION 6 /* LED1 + LED2 */ +// #define LED_PANIC 7 /* LED1 + LED2 */ + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + * UART0TX: GPIO0 + * UART0RX: GPIO1 + * UART1TX: GPIO8 + * UART1RX: GPIO9 + */ +#define CONFIG_RP2040_UART0_GPIO 0 /* TELEM */ + +#define CONFIG_RP2040_UART1_GPIO 8 /* GPS */ + +/* + * I2C (external) + * + * I2C1SCL: GPIO7 + * I2C1SDA: GPIO6 + * + * TODO: + * 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 CONFIG_RP2040_I2C1_GPIO 6 + +/* SPI0: + * SPIDEV_FLASH (probably micro sd card) + * CS: GPIO5 -- should be configured in sec/spi.cpp (probably) + * CLK: GPIO2 + * MISO: GPIO4 + * MOSI: GPIO3 + */ + +#define GPIO_SPI0_SCLK ( 2 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) +#define GPIO_SPI0_MISO ( 4 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) +#define GPIO_SPI0_MOSI ( 3 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) + +/* SPI1: + * MPU9250 and BMP280 + * CS: GPIO13 for MPU9250, GPIO14 for BMP280 -- should be configured in sec/spi.cpp (probably) + * CLK: GPIO10 + * MISO: GPIO12 + * MOSI: GPIO11 + */ + +#define GPIO_SPI1_SCLK ( 10 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) +#define GPIO_SPI1_MISO ( 12 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) +#define GPIO_SPI1_MOSI ( 11 | GPIO_FUN(RP2040_GPIO_FUNC_SPI) ) + + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..58d939dda3f9 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig @@ -0,0 +1,116 @@ +# +# 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_RP2040_SPI_DRIVER is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/raspberrypi/pico/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="rp2040" +CONFIG_ARCH_CHIP_RP2040=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_LOOPSPERMSEC=10450 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTSTR="PX4 RaspberryPi Pico" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=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_REGISTER=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_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y +CONFIG_NSH_DISABLE_REBOOT=y +CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_FATDEVNO=0 +CONFIG_NSH_FILEIOSIZE=512 +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_USBCONSOLE=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=270336 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RP2040_I2C1=y +CONFIG_RP2040_I2C=y +CONFIG_RP2040_SPI0=y +CONFIG_RP2040_SPI1=y +CONFIG_RP2040_SPI=y +CONFIG_RP2040_UART1=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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=32 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y diff --git a/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld b/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld new file mode 100644 index 000000000000..1193a1f5d7f9 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/scripts/flash.ld @@ -0,0 +1,113 @@ +/**************************************************************************** + * boards/arm/rp2040/raspberrypi-pico/scripts/raspberrypi-pico-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. + * + ****************************************************************************/ + +MEMORY +{ + flash (rx) : ORIGIN = 0x10000000, LENGTH = 2048K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 264K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) +EXTERN(bootStr) + +SECTIONS +{ + .flash_begin : { + __flash_binary_start = .; + } > flash + + .boot2 : { + __boot2_start__ = .; + KEEP (*(.boot2)) + __boot2_end__ = .; + } > flash + + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.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_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(.); + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > sram + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _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/raspberrypi/pico/nuttx-config/scripts/script.ld b/boards/raspberrypi/pico/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..1193a1f5d7f9 --- /dev/null +++ b/boards/raspberrypi/pico/nuttx-config/scripts/script.ld @@ -0,0 +1,113 @@ +/**************************************************************************** + * boards/arm/rp2040/raspberrypi-pico/scripts/raspberrypi-pico-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. + * + ****************************************************************************/ + +MEMORY +{ + flash (rx) : ORIGIN = 0x10000000, LENGTH = 2048K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 264K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) +EXTERN(bootStr) + +SECTIONS +{ + .flash_begin : { + __flash_binary_start = .; + } > flash + + .boot2 : { + __boot2_start__ = .; + KEEP (*(.boot2)) + __boot2_end__ = .; + } > flash + + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.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_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(.); + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > sram + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _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/raspberrypi/pico/src/CMakeLists.txt b/boards/raspberrypi/pico/src/CMakeLists.txt new file mode 100644 index 000000000000..b2cb070951a9 --- /dev/null +++ b/boards/raspberrypi/pico/src/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(drivers_board + boot_string.c + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c +) + +target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/raspberrypi/pico/src/board_config.h b/boards/raspberrypi/pico/src/board_config.h new file mode 100644 index 000000000000..581233e7ac36 --- /dev/null +++ b/boards/raspberrypi/pico/src/board_config.h @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 + +/* LEDs */ +// LED1 - GPIO 25 - Green +#define GPIO_LED1 PX4_MAKE_GPIO_OUTPUT_CLEAR(25) // Take a look at rpi_common micro_hal.h +#define GPIO_LED_BLUE GPIO_LED1 + +#define BOARD_OVERLOAD_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 + */ +#define ADC_CHANNELS (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3) // Change this later based on the adc channels actually used + +#define ADC_BATTERY_VOLTAGE_CHANNEL 1 // Corresponding GPIO 27. Used in init.c for disabling GPIO_IE +#define ADC_BATTERY_CURRENT_CHANNEL 2 // Corresponding GPIO 28. Used in init.c for disabling GPIO_IE +#define ADC_RC_RSSI_CHANNEL 0 + +/* High-resolution timer */ +#define HRT_TIMER 1 +#define HRT_TIMER_CHANNEL 1 +#define HRT_PPM_CHANNEL 1 // Number really doesn't matter for this board +#define GPIO_PPM_IN (16 | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) +#define RC_SERIAL_PORT "/dev/ttyS3" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* This board provides a DMA pool and APIs */ // Needs to be figured out +#define BOARD_DMA_ALLOC_POOL_SIZE 2048 + +#define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_CONSOLE_BUFFER_SIZE (1024*3) + +/* USB + * + * VBUS detection is on 29 ADC_DPM0 and PTE8 + */ +#define GPIO_USB_VBUS_VALID (24 | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) // Used in usb.c + +/* PWM + * + * Alternatively CH3/CH4 could be assigned to UART6_TX/RX + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 4 + +// Has pwm outputs +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +/* + * 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_USB_VBUS_VALID)) + +__BEGIN_DECLS + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Name: rp2040_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void rp2040_spiinitialize(void); + + +/**************************************************************************************************** + * Name: rp2040_usbinitialize + * + * Description: + * Called to configure USB IO. + * + ****************************************************************************************************/ + +extern void rp2040_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/raspberrypi/pico/src/boot_string.c b/boards/raspberrypi/pico/src/boot_string.c new file mode 100644 index 000000000000..3b3b2ec4b244 --- /dev/null +++ b/boards/raspberrypi/pico/src/boot_string.c @@ -0,0 +1,20 @@ +#include + +__attribute__((section(".boot2"))) +const char bootStr[256] = {0x00, 0xb5, 0x32, 0x4b, 0x21, 0x20, 0x58, 0x60, 0x98, 0x68, 0x02, 0x21, 0x88, 0x43, 0x98, 0x60, + 0xd8, 0x60, 0x18, 0x61, 0x58, 0x61, 0x2e, 0x4b, 0x00, 0x21, 0x99, 0x60, 0x02, 0x21, 0x59, 0x61, + 0x01, 0x21, 0xf0, 0x22, 0x99, 0x50, 0x2b, 0x49, 0x19, 0x60, 0x01, 0x21, 0x99, 0x60, 0x35, 0x20, + 0x00, 0xf0, 0x44, 0xf8, 0x02, 0x22, 0x90, 0x42, 0x14, 0xd0, 0x06, 0x21, 0x19, 0x66, 0x00, 0xf0, + 0x34, 0xf8, 0x19, 0x6e, 0x01, 0x21, 0x19, 0x66, 0x00, 0x20, 0x18, 0x66, 0x1a, 0x66, 0x00, 0xf0, + 0x2c, 0xf8, 0x19, 0x6e, 0x19, 0x6e, 0x19, 0x6e, 0x05, 0x20, 0x00, 0xf0, 0x2f, 0xf8, 0x01, 0x21, + 0x08, 0x42, 0xf9, 0xd1, 0x00, 0x21, 0x99, 0x60, 0x1b, 0x49, 0x19, 0x60, 0x00, 0x21, 0x59, 0x60, + 0x1a, 0x49, 0x1b, 0x48, 0x01, 0x60, 0x01, 0x21, 0x99, 0x60, 0xeb, 0x21, 0x19, 0x66, 0xa0, 0x21, + 0x19, 0x66, 0x00, 0xf0, 0x12, 0xf8, 0x00, 0x21, 0x99, 0x60, 0x16, 0x49, 0x14, 0x48, 0x01, 0x60, + 0x01, 0x21, 0x99, 0x60, 0x01, 0xbc, 0x00, 0x28, 0x00, 0xd0, 0x00, 0x47, 0x12, 0x48, 0x13, 0x49, + 0x08, 0x60, 0x03, 0xc8, 0x80, 0xf3, 0x08, 0x88, 0x08, 0x47, 0x03, 0xb5, 0x99, 0x6a, 0x04, 0x20, + 0x01, 0x42, 0xfb, 0xd0, 0x01, 0x20, 0x01, 0x42, 0xf8, 0xd1, 0x03, 0xbd, 0x02, 0xb5, 0x18, 0x66, + 0x18, 0x66, 0xff, 0xf7, 0xf2, 0xff, 0x18, 0x6e, 0x18, 0x6e, 0x02, 0xbd, 0x00, 0x00, 0x02, 0x40, + 0x00, 0x00, 0x00, 0x18, 0x00, 0x00, 0x07, 0x00, 0x00, 0x03, 0x5f, 0x00, 0x21, 0x22, 0x00, 0x00, + 0xf4, 0x00, 0x00, 0x18, 0x22, 0x20, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x10, 0x08, 0xed, 0x00, 0xe0, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x74, 0xb2, 0x4e, 0x7a + }; diff --git a/boards/raspberrypi/pico/src/i2c.cpp b/boards/raspberrypi/pico/src/i2c.cpp new file mode 100644 index 000000000000..4ab0a07f679c --- /dev/null +++ b/boards/raspberrypi/pico/src/i2c.cpp @@ -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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(2), +}; diff --git a/boards/raspberrypi/pico/src/init.c b/boards/raspberrypi/pico/src/init.c new file mode 100644 index 000000000000..f66a91ced0d3 --- /dev/null +++ b/boards/raspberrypi/pico/src/init.c @@ -0,0 +1,395 @@ +/**************************************************************************** + * + * 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. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "board_config.h" +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/** + * 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 + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * 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) +{ + // Configure the GPIO pins to outputs and keep them low. + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + 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. + */ + + if (status >= 0) { + up_mdelay(400); + } +} + +/************************************************************************************ + * 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 BOARD_ADC_USB_CONNECTED ? 0 : 1; +} + +/**************************************************************************** + * Name: rp2040_boardearlyinitialize + * + * Description: + * + * This function is taken directly from nuttx's rp2040_boardinitialize.c + ****************************************************************************/ + +void rp2040_boardearlyinitialize(void) +{ + /* Set default UART pin */ +#if defined(CONFIG_RP2040_UART0) && CONFIG_RP2040_UART0_GPIO >= 0 + rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO, RP2040_GPIO_FUNC_UART); /* TX */ + rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 1, RP2040_GPIO_FUNC_UART); /* RX */ +# ifdef CONFIG_SERIAL_OFLOWCONTROL + rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 2, RP2040_GPIO_FUNC_UART); /* CTS */ +# endif /* CONFIG_SERIAL_OFLOWCONTROL */ +# ifdef CONFIG_SERIAL_IFLOWCONTROL + rp2040_gpio_set_function(CONFIG_RP2040_UART0_GPIO + 3, RP2040_GPIO_FUNC_UART); /* RTS */ +# endif /* CONFIG_SERIAL_IFLOWCONTROL */ +#endif + +#if defined(CONFIG_RP2040_UART1) && CONFIG_RP2040_UART1_GPIO >= 0 + rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO, RP2040_GPIO_FUNC_UART); /* TX */ + rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 1, RP2040_GPIO_FUNC_UART); /* RX */ +# ifdef CONFIG_SERIAL_OFLOWCONTROL + rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 2, RP2040_GPIO_FUNC_UART); /* CTS */ +# endif /* CONFIG_SERIAL_OFLOWCONTROL */ +# ifdef CONFIG_SERIAL_IFLOWCONTROL + rp2040_gpio_set_function(CONFIG_RP2040_UART1_GPIO + 3, RP2040_GPIO_FUNC_UART); /* RTS */ +# endif /* CONFIG_SERIAL_IFLOWCONTROL */ +#endif +} + +/************************************************************************************ + * Name: rp2040_boardinitialize + * + * Description: + * All 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 +rp2040_boardinitialize(void) +{ + // /* Reset all PWM to Low outputs */ + // board_on_reset(-1); + + // /* configure LEDs */ + board_autoled_initialize(); + + // Disable IE and enable OD on GPIO 26-29 (These are ADC Pins) + // Do this only for the channels configured in board_config.h + rp2040_gpioconfig(27 | GPIO_FUN(RP2040_GPIO_FUNC_NULL)); /* BATT_VOLTAGE_SENS */ + clrbits_reg32(RP2040_PADS_BANK0_GPIO_IE, RP2040_PADS_BANK0_GPIO(27)); /* BATT_VOLTAGE_SENS */ + setbits_reg32(RP2040_PADS_BANK0_GPIO_OD, RP2040_PADS_BANK0_GPIO(27)); /* BATT_VOLTAGE_SENS */ + rp2040_gpioconfig(28 | GPIO_FUN(RP2040_GPIO_FUNC_NULL)); /* BATT_VOLTAGE_SENS */ + clrbits_reg32(RP2040_PADS_BANK0_GPIO_IE, RP2040_PADS_BANK0_GPIO(28)); /* BATT_CURRENT_SENS */ + setbits_reg32(RP2040_PADS_BANK0_GPIO_OD, RP2040_PADS_BANK0_GPIO(28)); /* BATT_CURRENT_SENS */ + + /* Set default I2C pin */ +#if defined(CONFIG_RP2040_I2C0) && CONFIG_RP2040_I2C0_GPIO >= 0 + rp2040_gpio_set_function(CONFIG_RP2040_I2C0_GPIO, RP2040_GPIO_FUNC_I2C); /* SDA */ + rp2040_gpio_set_function(CONFIG_RP2040_I2C0_GPIO + 1, RP2040_GPIO_FUNC_I2C); /* SCL */ + rp2040_gpio_set_pulls(CONFIG_RP2040_I2C0_GPIO, true, false); /* Pull up */ + rp2040_gpio_set_pulls(CONFIG_RP2040_I2C0_GPIO + 1, true, false); +#endif + +#if defined(CONFIG_RP2040_I2C1) && CONFIG_RP2040_I2C1_GPIO >= 0 + rp2040_gpio_set_function(CONFIG_RP2040_I2C1_GPIO, RP2040_GPIO_FUNC_I2C); /* SDA */ + rp2040_gpio_set_function(CONFIG_RP2040_I2C1_GPIO + 1, RP2040_GPIO_FUNC_I2C); /* SCL */ + rp2040_gpio_set_pulls(CONFIG_RP2040_I2C1_GPIO, true, false); /* Pull up */ + rp2040_gpio_set_pulls(CONFIG_RP2040_I2C1_GPIO + 1, true, false); +#endif + + // // TODO: power peripherals + // ///* configure power supply control/sense pins */ + // //stm32_configgpio(GPIO_PERIPH_3V3_EN); + // //stm32_configgpio(GPIO_VDD_BRICK_VALID); + // //stm32_configgpio(GPIO_VDD_USB_VALID); + + // // TODO: 3v3 Sensor? + // ///* Start with Sensor voltage off We will enable it + // // * in board_app_initialize + // // */ + // //stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + + // // TODO: SBUS inversion? SPEK power? + // //stm32_configgpio(GPIO_SBUS_INV); + // //stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + // // TODO: $$$ Unused? + // //stm32_configgpio(GPIO_8266_GPIO0); + // //stm32_configgpio(GPIO_8266_PD); + // //stm32_configgpio(GPIO_8266_RST); + + // /* Safety - led don in led driver */ + + // // TODO: unused? + // //stm32_configgpio(GPIO_BTN_SAFETY); + + // // TODO: RSSI + // //stm32_configgpio(GPIO_RSSI_IN); + + // stm32_configgpio(GPIO_PPM_IN); + + /* configure SPI all interfaces GPIO */ + rp2040_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. + * + ****************************************************************************/ + +// static struct spi_dev_s *spi1; +static struct spi_dev_s *spi2; + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + + /* configure the DMA allocator */ // Needs to be figured out + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "DMA alloc FAILED\n"); + } + + /* set up the serial DMA polling */ // RP2040 nuttx implementation doesn't have serial_dma_poll function yet. + // static struct hrt_call serial_dma_call; + // struct timespec ts; + + // /* + // * Poll at 1ms intervals for received bytes that have not triggered + // * a DMA event. + // */ + // ts.tv_sec = 0; + // ts.tv_nsec = 1000000; + + // hrt_call_every(&serial_dma_call, + // ts_to_abstime(&ts), + // ts_to_abstime(&ts), + // (hrt_callout)stm32_serial_dma_poll, + // NULL); + + /* initial LED state */ + drv_led_start(); + led_on(LED_BLUE); + + // if (board_hardfault_init(2, true) != 0) { // Needs to be figured out as RP2040 doesn't have BBSRAM. + // led_off(LED_BLUE); + // } + + + /* Configure SPI-based devices */ + + // // SPI1: SDCard // Will be configured later + // /* Get the SPI port for the microSD slot */ + // spi1 = rp2040_spibus_initialize(CONFIG_NSH_MMCSDSPIPORTNO); // PX4_BUS_NUMBER_FROM_PX4(1) + + // if (!spi1) { + // syslog(LOG_ERR, "[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + // led_off(LED_BLUE); + // } + + // /* Now bind the SPI interface to the MMCSD driver */ + // int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi1); + + // if (result != OK) { + // led_off(LED_BLUE); + // syslog(LOG_ERR, "[boot] FAILED to bind SPI port 1 to the MMCSD driver\n"); + // } + + // up_udelay(20); + + // SPI2: MPU9250 and BMP280 + spi2 = rp2040_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(2)); + + if (!spi2) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); + led_off(LED_BLUE); + } + + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + up_udelay(20); + +// #if defined(FLASH_BASED_PARAMS) // This probably doesn't relate to RP2040 right now. +// static sector_descriptor_t params_sector_map[] = { +// {1, 16 * 1024, 0x08004000}, +// {0, 0, 0}, +// }; + +// /* Initialize the flashfs layer to use heap allocated memory */ +// 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_off(LED_AMBER); +// } + +// #endif + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/raspberrypi/pico/src/led.c b/boards/raspberrypi/pico/src/led.c new file mode 100644 index 000000000000..2e92025b4be9 --- /dev/null +++ b/boards/raspberrypi/pico/src/led.c @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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 "board_config.h" + +#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 + +static uint32_t g_ledmap[] = { + GPIO_LED_BLUE, // Onboard led on raspberrypi pico +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + px4_arch_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + if (led == 0) { + px4_arch_gpiowrite(g_ledmap[led], state); + } +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + if (led == 0) { + phy_set_led(led, !px4_arch_gpioread(g_ledmap[led])); + } +} + +// __EXPORT void board_autoled_initialize() +// { +// /* Configure LED1 GPIO for output */ +// px4_arch_configgpio(GPIO_LED1); +// } + +// __EXPORT void board_autoled_on(int led) +// { +// if (led == 1) { +// /* Pull down to switch on */ +// px4_arch_gpiowrite(GPIO_LED1, false); +// } +// } + +// __EXPORT void board_autoled_off(int led) +// { +// if (led == 1) { +// /* Pull up to switch off */ +// px4_arch_gpiowrite(GPIO_LED1, true); +// } +// } diff --git a/boards/raspberrypi/pico/src/spi.cpp b/boards/raspberrypi/pico/src/spi.cpp new file mode 100644 index 000000000000..a258dec42dd8 --- /dev/null +++ b/boards/raspberrypi/pico/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::SPI0, { + initSPIDevice(SPIDEV_MMCSD(0), SPI::CS{GPIO::Pin5}), + }), + initSPIBusExternal(SPI::Bus::SPI1, { + initSPIConfigExternal(SPI::CS{GPIO::Pin13}), + initSPIConfigExternal(SPI::CS{GPIO::Pin14}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/raspberrypi/pico/src/timer_config.cpp b/boards/raspberrypi/pico/src/timer_config.cpp new file mode 100644 index 000000000000..4cf036efd4a1 --- /dev/null +++ b/boards/raspberrypi/pico/src/timer_config.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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1), + initIOTimer(Timer::Timer2), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::ChannelA}, {GPIO::Pin18}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::ChannelB}, {GPIO::Pin19}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::ChannelA}, {GPIO::Pin20}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::ChannelB}, {GPIO::Pin21}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers, + timer_io_channels); diff --git a/boards/raspberrypi/pico/src/usb.c b/boards/raspberrypi/pico/src/usb.c new file mode 100644 index 000000000000..393f80960e33 --- /dev/null +++ b/boards/raspberrypi/pico/src/usb.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include "board_config.h" + +/************************************************************************************ + * Name: rp2040_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the omnibusf4sd board. + * + ************************************************************************************/ + +__EXPORT void rp2040_usbinitialize(void) +{ + px4_arch_configgpio(GPIO_USB_VBUS_VALID); +} + +/************************************************************************************ + * 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 rp2040_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + diff --git a/boards/scumaker/pilotpi/arm64.cmake b/boards/scumaker/pilotpi/arm64.cmake deleted file mode 100644 index e19df827f41c..000000000000 --- a/boards/scumaker/pilotpi/arm64.cmake +++ /dev/null @@ -1,99 +0,0 @@ -add_definitions( - -D__PX4_LINUX -) - -px4_add_board( - VENDOR scumaker - MODEL pilotpi - LABEL arm64 - PLATFORM posix - ARCHITECTURE cortex-a53 - ROMFSROOT px4fmu_common - TOOLCHAIN aarch64-linux-gnu - TESTING - DRIVERS - adc/ads1115 - #barometer # all available barometer drivers - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - imu/invensense/icm42688p - imu/invensense/icm42605 - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - magnetometer/isentek/ist8310 - magnetometer/qmc5883l - pca9685_pwm_out - pwm_out_sim - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - temperature_compensation - sih - #simulator - vmount - vtol_att_control - SYSTEMCMDS - dyn - esc_calib - led_control - mixer - motor_ramp - param - perf - pwm - sd_bench - shutdown - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/scumaker/pilotpi/arm64.px4board b/boards/scumaker/pilotpi/arm64.px4board new file mode 100644 index 000000000000..756dd57aae6c --- /dev/null +++ b/boards/scumaker/pilotpi/arm64.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" +CONFIG_EXAMPLES_FAKE_IMU=y +CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y diff --git a/boards/scumaker/pilotpi/cmake/upload.cmake b/boards/scumaker/pilotpi/cmake/upload.cmake index d9a75e97ed9f..f3ff69c4cbe3 100644 --- a/boards/scumaker/pilotpi/cmake/upload.cmake +++ b/boards/scumaker/pilotpi/cmake/upload.cmake @@ -46,7 +46,7 @@ endif() add_custom_target(upload COMMAND rsync -arh --progress ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/pilotpi*.config ${PX4_BINARY_DIR}/etc # source - \$\{AUTOPILOT_USER\}@\$\{AUTOPILOT_HOST\}:/home/\$\{AUTOPILOT_USER\}/px4 # destination + "${AUTOPILOT_USER}@${AUTOPILOT_HOST}:/home/${AUTOPILOT_USER}/px4" # destination DEPENDS px4 COMMENT "uploading px4" USES_TERMINAL diff --git a/boards/scumaker/pilotpi/default.cmake b/boards/scumaker/pilotpi/default.cmake deleted file mode 100644 index de0486cf2002..000000000000 --- a/boards/scumaker/pilotpi/default.cmake +++ /dev/null @@ -1,99 +0,0 @@ -add_definitions( - -D__PX4_LINUX -) - -px4_add_board( - VENDOR scumaker - MODEL pilotpi - LABEL default - PLATFORM posix - ARCHITECTURE cortex-a53 - ROMFSROOT px4fmu_common - TOOLCHAIN arm-linux-gnueabihf - TESTING - DRIVERS - adc/ads1115 - #barometer # all available barometer drivers - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #imu # all available imu drivers - imu/invensense/icm42688p - imu/invensense/icm42605 - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - magnetometer/isentek/ist8310 - magnetometer/qmc5883l - pca9685_pwm_out - pwm_out_sim - rc_input - #telemetry # all available telemetry drivers - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - temperature_compensation - sih - #simulator - vmount - vtol_att_control - SYSTEMCMDS - dyn - esc_calib - led_control - mixer - motor_ramp - param - perf - pwm - sd_bench - shutdown - tests # tests and test runner - #top - topic_listener - tune_control - uorb - ver - work_queue - EXAMPLES - dyn_hello # dynamically loading modules example - fake_gps - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - #hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board new file mode 100644 index 000000000000..21cf686403ac --- /dev/null +++ b/boards/scumaker/pilotpi/default.px4board @@ -0,0 +1,79 @@ +CONFIG_PLATFORM_POSIX=y +CONFIG_BOARD_LINUX=y +CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf" +CONFIG_BOARD_ARCHITECTURE="cortex-a53" +CONFIG_BOARD_TESTING=y +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=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_L1=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_DYN=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SHUTDOWN=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_DYN_HELLO=y +CONFIG_EXAMPLES_FAKE_GPS=y +CONFIG_EXAMPLES_FIXEDWING_CONTROL=y +CONFIG_EXAMPLES_HELLO=y +CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y +CONFIG_EXAMPLES_PX4_SIMPLE_APP=y +CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y +CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y +CONFIG_EXAMPLES_WORK_ITEM=y diff --git a/boards/scumaker/pilotpi/init/rc.board_defaults b/boards/scumaker/pilotpi/init/rc.board_defaults new file mode 100644 index 000000000000..1677fa4d8298 --- /dev/null +++ b/boards/scumaker/pilotpi/init/rc.board_defaults @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# 1K + 4.7K +param set-default BAT1_V_DIV 5.7 diff --git a/boards/scumaker/pilotpi/src/board_config.h b/boards/scumaker/pilotpi/src/board_config.h index 4ce9b73e3825..76abae225178 100644 --- a/boards/scumaker/pilotpi/src/board_config.h +++ b/boards/scumaker/pilotpi/src/board_config.h @@ -42,16 +42,20 @@ #define BOARD_OVERRIDE_UUID "RPIID00000000000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_RPI -/* - * I2C busses - */ + +// I2C +#define CONFIG_I2C 1 #define PX4_NUMBER_I2C_BUSES 2 + +// SPI +#define CONFIG_SPI 1 + + #define ADC_BATTERY_VOLTAGE_CHANNEL 0 #define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 2 -#define BOARD_BATTERY1_V_DIV 5.7f // 1K + 4.7K #define ADC_DP_V_DIV 1.0f #define BOARD_ADC_OPEN_CIRCUIT_V 5.3f // Powered from USB diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board new file mode 100644 index 000000000000..125123fad510 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -0,0 +1,109 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=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_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_DRIVERS_RPM=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_L1=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_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/sky-drones/smartap-airlink/extras/px4_io-v2_default.bin b/boards/sky-drones/smartap-airlink/extras/px4_io-v2_default.bin new file mode 100755 index 000000000000..25a00e2357d7 Binary files /dev/null and b/boards/sky-drones/smartap-airlink/extras/px4_io-v2_default.bin differ diff --git a/boards/sky-drones/smartap-airlink/extras/sky-drones_smartap-airlink_bootlader.bin b/boards/sky-drones/smartap-airlink/extras/sky-drones_smartap-airlink_bootlader.bin new file mode 100755 index 000000000000..8651520f6bbd Binary files /dev/null and b/boards/sky-drones/smartap-airlink/extras/sky-drones_smartap-airlink_bootlader.bin differ diff --git a/boards/sky-drones/smartap-airlink/firmware.prototype b/boards/sky-drones/smartap-airlink/firmware.prototype new file mode 100644 index 000000000000..96f17fbf377b --- /dev/null +++ b/boards/sky-drones/smartap-airlink/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 55, + "magic": "PX4FWv1", + "description": "Firmware for the SMARTAP AIRLINK board", + "image": "", + "build_time": 0, + "summary": "SMARTAP-AIRLINK", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2064384, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_defaults b/boards/sky-drones/smartap-airlink/init/rc.board_defaults new file mode 100644 index 000000000000..14270fc9df21 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/init/rc.board_defaults @@ -0,0 +1,31 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# Telemetry for onboard computer +param set-default MAV_1_CONFIG 102 # TELEM2 +param set-default MAV_1_MODE 0 # Normal +param set-default SER_TEL2_BAUD 921600 # 921600 + +# Temperature stabilization +param set-default SENS_EN_THERMAL 1 # Enable heater +param set-default SENS_TEMP_ID 2359314 # Heated IMU ID + +# Battery scaling +param set-default BAT_N_CELLS 4 +param set-default BAT1_N_CELLS 4 + +param set-default BAT1_V_DIV 15.51 + +param set-default BAT1_A_PER_V 36.00 + +param set-default BAT_V_OFFS_CURR 0.413 + +# Disable safety switch +param set-default CBRK_IO_SAFETY 22027 + + +safety_button start + +set LOGGER_BUF 32 diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_sensors b/boards/sky-drones/smartap-airlink/init/rc.board_sensors new file mode 100644 index 000000000000..5ce477bf181d --- /dev/null +++ b/boards/sky-drones/smartap-airlink/init/rc.board_sensors @@ -0,0 +1,34 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal SPI bus MPU-9250 / SPI 2 [FPC] +mpu9250 -s -b 2 -R 6 start + +# Internal SPI bus ICM-20602 / SPI 1 +icm20602 -R 0 -s start + +# Internal SPI bus MPU-9250 / SPI 3 +mpu9250 -s -b 3 -M -R 4 start + +# Internal compass +bmm150 -I start + +# Internal Baro +ms5611 -X start + +# Internal Baro +bmp388 -I -a 0x77 start +#bmp388 -I start + +# External HMC5983 +hmc5883 -T -X -R 4 start + +# External LIS3MDL +lis3mdl -R 2 -X start + +# NCP5623 Led driver +rgbled_ncp5623c -X -a 0x38 start + diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/Kconfig b/boards/sky-drones/smartap-airlink/nuttx-config/Kconfig new file mode 100644 index 000000000000..520c5abadb6f --- /dev/null +++ b/boards/sky-drones/smartap-airlink/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-8, CAP1 as PROBE_1-9 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-8, CAP1 as PROBE_1-9" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-8, CAP1 to provide timing signals from selected drivers. diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/include/board.h b/boards/sky-drones/smartap-airlink/nuttx-config/include/board.h new file mode 100644 index 000000000000..6e5759beb117 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/nuttx-config/include/board.h @@ -0,0 +1,503 @@ +/************************************************************************************ + * nuttx-configs/smartap-airlink/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_SMARTAP_AIRLINK_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_SMARTAP_AIRLINK_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 board provides the following clock sources: + * + * X301: 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 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 2 <= PLLM <= 63 + * 192 <= PLLN <= 432 + * 192 MHz <= PLL_VCO <= 432MHz + * + * SYSCLK = PLL_VCO / PLLP + * Subject to + * + * PLLP = {2, 4, 6, 8} + * SYSCLK <= 216 MHz + * + * USB OTG FS, SDMMC and RNG Clock = PLL_VCO / PLLQ + * Subject to + * The USB OTG FS requires a 48 MHz clock to work correctly. The SDMMC + * and the random number generator need a frequency lower than or equal + * to 48 MHz to work correctly. + * + * 2 <= PLLQ <= 15 + */ + +/* Highest SYSCLK with USB OTG FS clock = 48 MHz + * + * PLL_VCO = (16,000,000 / 8) * 216 = 432 MHz + * SYSCLK = 432 MHz / 2 = 216 MHz + * USB OTG FS, SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(216) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(9) + +#define STM32_VCO_FREQUENCY ((STM32_HSE_FREQUENCY / 8) * 216) +#define STM32_SYSCLK_FREQUENCY (STM32_VCO_FREQUENCY / 2) +#define STM32_OTGFS_FREQUENCY (STM32_VCO_FREQUENCY / 9) + +/* Configure factors for PLLSAI clock */ + +#define CONFIG_STM32F7_PLLSAI 1 +#define STM32_RCC_PLLSAICFGR_PLLSAIN RCC_PLLSAICFGR_PLLSAIN(192) +#define STM32_RCC_PLLSAICFGR_PLLSAIP RCC_PLLSAICFGR_PLLSAIP(8) +#define STM32_RCC_PLLSAICFGR_PLLSAIQ RCC_PLLSAICFGR_PLLSAIQ(4) +#define STM32_RCC_PLLSAICFGR_PLLSAIR RCC_PLLSAICFGR_PLLSAIR(2) + +/* Configure Dedicated Clock Configuration Register */ + +#define STM32_RCC_DCKCFGR1_PLLI2SDIVQ RCC_DCKCFGR1_PLLI2SDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVQ RCC_DCKCFGR1_PLLSAIDIVQ(1) +#define STM32_RCC_DCKCFGR1_PLLSAIDIVR RCC_DCKCFGR1_PLLSAIDIVR(0) +#define STM32_RCC_DCKCFGR1_SAI1SRC RCC_DCKCFGR1_SAI1SEL(0) +#define STM32_RCC_DCKCFGR1_SAI2SRC RCC_DCKCFGR1_SAI2SEL(0) +#define STM32_RCC_DCKCFGR1_TIMPRESRC 0 +#define STM32_RCC_DCKCFGR1_DFSDM1SRC 0 +#define STM32_RCC_DCKCFGR1_ADFSDM1SRC 0 + + + +/* Configure factors for PLLI2S clock */ + +#define CONFIG_STM32F7_PLLI2S 1 +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SP RCC_PLLI2SCFGR_PLLI2SP(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) + +/* Configure Dedicated Clock Configuration Register 2 */ + +#define STM32_RCC_DCKCFGR2_USART1SRC RCC_DCKCFGR2_USART1SEL_APB +#define STM32_RCC_DCKCFGR2_USART2SRC RCC_DCKCFGR2_USART2SEL_APB +#define STM32_RCC_DCKCFGR2_UART4SRC RCC_DCKCFGR2_UART4SEL_APB +#define STM32_RCC_DCKCFGR2_UART5SRC RCC_DCKCFGR2_UART5SEL_APB +#define STM32_RCC_DCKCFGR2_USART6SRC RCC_DCKCFGR2_USART6SEL_APB +#define STM32_RCC_DCKCFGR2_UART7SRC RCC_DCKCFGR2_UART7SEL_APB +#define STM32_RCC_DCKCFGR2_UART8SRC RCC_DCKCFGR2_UART8SEL_APB +#define STM32_RCC_DCKCFGR2_I2C1SRC RCC_DCKCFGR2_I2C1SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C2SRC RCC_DCKCFGR2_I2C2SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C3SRC RCC_DCKCFGR2_I2C3SEL_HSI +#define STM32_RCC_DCKCFGR2_I2C4SRC RCC_DCKCFGR2_I2C4SEL_HSI +#define STM32_RCC_DCKCFGR2_LPTIM1SRC RCC_DCKCFGR2_LPTIM1SEL_APB +#define STM32_RCC_DCKCFGR2_CECSRC RCC_DCKCFGR2_CECSEL_HSI +#define STM32_RCC_DCKCFGR2_CK48MSRC RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_SDMMCSRC RCC_DCKCFGR2_SDMMCSEL_48MHZ +#define STM32_RCC_DCKCFGR2_SDMMC2SRC RCC_DCKCFGR2_SDMMC2SEL_48MHZ +#define STM32_RCC_DCKCFGR2_DSISRC RCC_DCKCFGR2_DSISEL_PHY + + +/* Several prescalers allow the configuration of the two AHB buses, the + * high-speed APB (APB2) and the low-speed APB (APB1) domains. The maximum + * frequency of the two AHB buses is 216 MHz while the maximum frequency of + * the high-speed APB domains is 108 MHz. The maximum allowed frequency of + * the low-speed APB domain is 54 MHz. + */ + +/* AHB clock (HCLK) is SYSCLK (216 MHz) */ + +#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/4 (54 MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* 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) + +/* APB2 clock (PCLK2) is HCLK/2 (108MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* 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_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* SDMMC dividers. Note that slower clocking is required when DMA is disabled + * in order to avoid RX overrun/TX underrun errors due to delayed responses + * to service FIFOs in interrupt driven mode. These values have not been + * tuned!!! + * + * SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(118+2)=400 KHz + */ + +/* Use the Falling edge of the SDIO_CLK clock to change the edge the + * data and commands are change on + */ + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +#define STM32_SDMMC_INIT_CLKDIV (118 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* DMA ON: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(1+2)=16 MHz + * DMA OFF: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(2+2)=12 MHz + */ + +#ifdef CONFIG_STM32F7_SDMMC_DMA +# define STM32_SDMMC_MMCXFR_CLKDIV (1 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +/* DMA ON: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(1+2)=16 MHz + * DMA OFF: SDMMCCLK=48MHz, SDMMC_CK=SDMMCCLK/(2+2)=12 MHz + */ +//TODO #warning "Check Freq for 24mHz" + +#ifdef CONFIG_STM32F7_SDMMC_DMA +# define STM32_SDMMC_SDXFR_CLKDIV (1 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (2 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +/* FLASH wait states + * + * --------- ---------- ----------- + * VDD MAX SYSCLK WAIT STATES + * --------- ---------- ----------- + * 1.7-2.1 V 180 MHz 8 + * 2.1-2.4 V 216 MHz 9 + * 2.4-2.7 V 216 MHz 8 + * 2.7-3.6 V 216 MHz 7 + * --------- ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 7 + +/* LED definitions ******************************************************************/ +/* The 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. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB15 */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB14 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 /* PD3 */ + +#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_5 /* PH14 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PH13 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_1 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_4 /* PB9 */ +#define GPIO_UART5_RTS GPIO_UART5_RTS_1 /* PC8 */ +#define GPIO_UART5_CTS GPIO_UART5_CTS_1 /* PC9 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC 7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_2 /* PF6 */ +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +/* UART8: has no remap + * + * GPIO_UART8_RX PE0 + * GPIO_UART8_TX PE1 + */ + + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_2 /* PB6 */ + +/* SPI + * SPI1 is sensors1 + * SPI2 is sensors2 + * SPI3 is sensors3 + * SPI4 is Not Used + * SPI5 is FRAM + * SPI6 is EXTERNAL1 + * + */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_2 /* PB4 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_2 /* PB5 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PI3 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_5 /* PI1 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 /* PC11 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_1 /* PB2 */ +#define GPIO_SPI3_SCK GPIO_SPI3_SCK_2 /* PC10 */ + +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF11 */ +#define GPIO_SPI5_SCK GPIO_SPI5_SCK_1 /* PF7 */ + +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_2 /* PA6 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */ +#define GPIO_SPI6_SCK 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_1 /* PB7 */ + +#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_PIN7) + +#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_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8) + +#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 PG9 + * SDMMC2_D1 PG10 + * SDMMC2_D2 PG11 + * SDMMC2_D3 PG12 + */ + +#define GPIO_SDMMC2_D0 GPIO_SDMMC2_D0_2 +#define GPIO_SDMMC2_D1 GPIO_SDMMC2_D1_2 +#define GPIO_SDMMC2_D2 GPIO_SDMMC2_D2_2 +#define GPIO_SDMMC2_D3 GPIO_SDMMC2_D3_2 + +/* The STM32 F7 connects to a TI DP83848TSQ/NOPB + * using RMII + * + * STM32 F7 BOARD DP83848TSQ/NOPB + * GPIO SIGNAL PIN NAME + * -------- ------------ ------------- + * PA7 ETH_CRS_DV CRS_DV + * PC1 ETH_MDC MDC + * PA2 ETH_MDIO MDIO + * PA1 ETH_REF_CL X1 + * PC4 ETH_RXD0 RX_D0 + * PC5 ETH_RXD1 RX_D1 + * PB11 ETH_TX_EN TX_EN + * PG13 ETH_TXD0 TX_D0 + * PB13 ETH_TXD1 TX_D1 + * + * The PHY address is 1, since COL/PHYAD0 features a pull up. + */ + +#define GPIO_ETH_RMII_TX_EN GPIO_ETH_RMII_TX_EN_1 +#define GPIO_ETH_RMII_TXD0 GPIO_ETH_RMII_TXD0_2 +#define GPIO_ETH_RMII_TXD1 GPIO_ETH_RMII_TXD1_1 + + +/* 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_PORTE|GPIO_PIN14) /* PE14 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN10) /* PA10 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) /* PA8 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_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) /* PI0 CAP1 */ + +# 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_SMARTAP_AIRLINK_INCLUDE_BOARD_H */ diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/include/board_dma_map.h b/boards/sky-drones/smartap-airlink/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..5722e135f959 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/nuttx-config/include/board_dma_map.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/* +| DMA1 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | SPI3_RX_1 | SPDIFRX_DT | SPI3_RX_2 | SPI2_RX | SPI2_TX | SPI3_TX_1 | SPDIFRX_CS | SPI3_TX_2 | +| Channel 1 | I2C1_RX | I2C3_RX | TIM7_UP_1 | - | TIM7_UP_2 | I2C1_RX_1 | I2C1_TX | I2C1_TX_1 | +| Channel 2 | TIM4_CH1 | - | I2C4_RX | TIM4_CH2 | - | I2C4_RX | TIM4_UP | TIM4_CH3 | +| Channel 3 | - | TIM2_UP_1 | I2C3_RX_1 | - | I2C3_TX | TIM2_CH1 | TIM2_CH2 | TIM2_UP_2 | +| | | TIM2_CH3 | | | | | TIM2_CH4_1 | TIM2_CH4_2 | +| Channel 4 | UART5_RX | USART3_RX | UART4_RX | USART3_TX_1 | UART4_TX | USART2_RX | USART2_TX | UART5_TX | +| Channel 5 | UART8_TX | UART7_TX | TIM3_CH4 | UART7_RX | TIM3_CH1 | TIM3_CH2 | UART8_RX | TIM3_CH3 | +| | | | TIM3_UP | | TIM3_TRIG | | | | +| Channel 6 | TIM5_CH3 | TIM5_CH4_1 | TIM5_CH1 | TIM5_CH4_2 | TIM5_CH2 | - | TIM5_UP_2 | - | +| | TIM5_UP_1 | TIM5_TRIG_1 | | TIM5_TRIG_2 | | | | | +| Channel 7 | - | TIM6_UP | I2C2_RX | I2C2_RX_1 | USART3_TX_2 | DAC1 | DAC2 | I2C2_TX | +| Channel 8 | I2C3_TX | I2C4_RX | - | - | I2C2_TX | - | I2C4_TX | - | +| Channel 9 | - | SPI2_RX | - | - | - | - | SPI2_TX | - | +| | | | | | | | | | +| Usage | UART5_RX | SPI2_RX | SPI3_RX_2 | UART7_RX | SPI2_TX | SPI3_TX_1 | TIM4_UP | UART5_TX | + + +| DMA2 | Stream 0 | Stream 1 | Stream 2 | Stream 3 | Stream 4 | Stream 5 | Stream 6 | Stream 7 | +|------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------|------------------| +| Channel 0 | ADC1_1 | SAI1_A | TIM8_CH1_1 | SAI1_A_1 | ADC1_2 | SAI1_B_1 | TIM1_CH1_1 | SAI2_B_2 | +| | | | TIM8_CH2_1 | | | | TIM1_CH2_1 | | +| | | | TIM8_CH3_1 | | | | TIM1_CH3_1 | | +| Channel 1 | - | DCMI_1 | ADC2_1 | ADC2_2 | SAI1_B | SPI6_TX | SPI6_RX | DCMI_2 | +| Channel 2 | ADC3_1 | ADC3_2 | - | SPI5_RX_1 | SPI5_TX_1 | CRYP_OUT | CRYP_IN | HASH_IN | +| Channel 3 | SPI1_RX_1 | - | SPI1_RX_2 | SPI1_TX_1 | SAI2_A | SPI1_TX_2 | SAI2_B | QUADSPI | +| Channel 4 | SPI4_RX_1 | SPI4_TX_1 | USART1_RX_1 | SDMMC1_1 | - | USART1_RX_2 | SDMMC1_2 | USART1_TX | +| Channel 5 | - | USART6_RX_1 | USART6_RX_2 | SPI4_RX_2 | SPI4_TX_2 | - | USART6_TX_1 | USART6_TX_2 | +| Channel 6 | TIM1_TRIG_1 | TIM1_CH1_2 | TIM1_CH2_2 | TIM1_CH1 | TIM1_CH4 | TIM1_UP | TIM1_CH3_2 | - | +| | | | | | TIM1_TRIG_2 | | | | +| | | | | | TIM1_COM | | | | +| Channel 7 | - | TIM8_UP | TIM8_CH1_2 | TIM8_CH2_2 | TIM8_CH3_2 | SPI5_RX_2 | SPI5_TX_2 | TIM8_CH4 | +| | | | | | | | | TIM8_TRIG | +| | | | | | | | | TIM8_COM | +| Channel 8 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | DSFDM1_FLT0 | DSFDM1_FLT1 | DSFDM1_FLT2 | DSFDM1_FLT3 | +| Channel 9 | JPEG_IN | JPEG_OUT | SPI4_TX | JPEG_IN | JPEG_OUT | SPI5_RX | - | - | +| Channel 10 | SAI1_B | SAI2_B | SAI2_A | - | - | - | SAI1_A | - | +| Channel 11 | SDMMC2 | - | QUADSPI | - | - | SDMMC2 | - | - | +| | | | | | | | | | +| Usage | SDMMC2 | USART6_RX_1 | SPI1_RX_2 | SPI1_TX_1 | | TIM1_UP | | USART6_TX_2 | + */ + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +// DMAMAP_UART5_RX // DMA1, Stream 0, Channel 4 (TELEM2 RX) +#define DMAMAP_SPI2_RX DMAMAP_SPI2_RX_1 // DMA1, Stream 1, Channel 9 (SPI2 RX) +#define DMAMAP_SPI3_RX DMAMAP_SPI3_RX_2 // DMA1, Stream 2, Channel 0 (SPI3 RX) +// DMAMAP_UART7_RX // DMA1, Stream 3, Channel 5 (TELEM1 RX) +#define DMAMAP_SPI2_TX DMAMAP_SPI2_TX_2 // DMA1, Stream 4, Channel 0 (SPI2 TX) +#define DMAMAP_SPI3_TX DMAMAP_SPI3_TX_1 // DMA1, Stream 5, Channel 0 (SPI3 TX) +// DMAMAP_TIM4_UP // DMA1, Stream 6, Channel 2 (DSHOT) +// DMAMAP_UART5_TX // DMA1, Stream 7, Channel 4 (TELEM2 TX) + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMAMAP_SDMMC2 DMAMAP_SDMMC2_1 // DMA2, Stream 0, Channel 11 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 // DMA2, Stream 1, Channel 5 (PX4IO) +#define DMAMAP_SPI1_RX DMAMAP_SPI1_RX_2 // DMA2, Stream 2, Channel 3 (SPI sensors RX) +#define DMAMAP_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3 (SPI sensors TX) +// AVAILABLE // DMA2, Stream 4 +// DMAMAP_TIM1_UP // DMA2, Stream 5, Channel 6 (DSHOT) +// AVAILABLE // DMA2, Stream 6 +#define DMAMAP_USART6_TX DMAMAP_USART6_TX_2 // DMA2, Stream 7, Channel 5 (PX4IO) diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..7017364cfff8 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig @@ -0,0 +1,288 @@ +# +# 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_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_ARP is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/sky-drones/smartap-airlink/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0037 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU SmartAP AIRLink" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x0483 +CONFIG_CDCACM_VENDORSTR="Sky-Drones" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_LAN8742A=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_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_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_BINARY=y +CONFIG_IPCFG_CHARDEV=y +CONFIG_IPCFG_PATH="/fs/mtd_net" +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +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=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSCLIENT_ENTRIES=8 +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +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_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_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +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_TELNET=y +CONFIG_NSH_TELNET_LOGIN=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_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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_STM32F7_ADC1=y +CONFIG_STM32F7_ADC3=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_ETHMAC=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PHYADDR=0 +CONFIG_STM32F7_PHYSR=31 +CONFIG_STM32F7_PHYSR_100MBPS=0x8 +CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10 +CONFIG_STM32F7_PHYSR_MODE=0x10 +CONFIG_STM32F7_PHYSR_SPEED=0x8 +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC2=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI2_DMA=y +CONFIG_STM32F7_SPI2_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_SPI3_DMA=y +CONFIG_STM32F7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMA=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM9=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART5=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=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=3000 +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_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_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +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_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/scripts/script.ld b/boards/sky-drones/smartap-airlink/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..8d5a78da6bfc --- /dev/null +++ b/boards/sky-drones/smartap-airlink/nuttx-config/scripts/script.ld @@ -0,0 +1,190 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 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. + * + ****************************************************************************/ + +/* The STM32F765IIT6 has 2048 KiB of main FLASH memory. This FLASH memory + * can be accessed from either the AXIM interface at address 0x0800:0000 or + * from the ITCM interface at address 0x0020:0000. + * + * Additional information, including the option bytes, is available at at + * FLASH at address 0x1ff0:0000 (AXIM) or 0x0010:0000 (ITCM). + * + * In the STM32F765IIT6, 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 on ITCM at 0x0020:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x0010:0000 + * + * NuttX does not modify these option byes. On the unmodified NUCLEO-144 + * board, the BOOT0 pin is at ground so by default, the STM32F765IIT6 will + * boot from address 0x0020:0000 in ITCM FLASH. + * + * The STM32F765IIT6 also has 512 KiB of data SRAM (in addition to ITCM SRAM). + * SRAM is split up into three blocks: + * + * 1) 128 KiB of DTCM SRM beginning at address 0x2000:0000 + * 2) 368 KiB of SRAM1 beginning at address 0x2002:0000 + * 3) 16 KiB of SRAM2 beginning at address 0x2007:c000 + * + * 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. + * + * Bootloader reserves the first 32K bank (2 Mbytes Flash memory single bank) + * organization (256 bits read width) + */ + +MEMORY +{ + FLASH_ITCM (rx) : ORIGIN = 0x00208000, LENGTH = 2016K + FLASH_AXIM (rx) : ORIGIN = 0x08008000, LENGTH = 2016K + + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 16K + DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K + SRAM2 (rwx) : ORIGIN = 0x2007c000, LENGTH = 16K +} + +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_AXIM + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH_AXIM + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > FLASH_AXIM + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH_AXIM + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH_AXIM + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > SRAM1 AT > FLASH_AXIM + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > SRAM1 + + /* 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) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH_AXIM + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/sky-drones/smartap-airlink/src/CMakeLists.txt b/boards/sky-drones/smartap-airlink/src/CMakeLists.txt new file mode 100644 index 000000000000..7afd265ecfd7 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/CMakeLists.txt @@ -0,0 +1,58 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(drivers_board + can.c + i2c.cpp + init.cpp + led.c + manifest.c + mtd.cpp + 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 # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h new file mode 100644 index 000000000000..024915c05509 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/board_config.h @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS5" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +#define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* 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 PC12 - nARMED - Trace Connector Pin 8 + + */ +#undef TRACE_PINS + +/* 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 +#endif + +/* I2C busses */ + +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_SE050 0x48 + +#define GPIO_I2C4_DRDY1_BMP388 /* PG5 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5) +#define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) + +/* + * 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 BOARD_NUMBER_BRICKS 1 + +#define ADC1_CH(n) (n) +#define ADC1_GPIO(n) GPIO_ADC1_IN##n +#define ADC3_CH(n) (n) +#define ADC3_GPIO(n) GPIO_ADC3_IN##n + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ ADC1_GPIO(0), \ + /* PA4 */ ADC1_GPIO(4), \ + /* PB0 */ ADC1_GPIO(8), \ + /* PB1 */ ADC1_GPIO(9), \ + /* PC0 */ ADC1_GPIO(10), \ + /* PC2 */ ADC1_GPIO(12), \ + /* PC3 */ ADC1_GPIO(13), \ + /* PF4 */ ADC3_GPIO(14), \ + /* PF5 */ ADC3_GPIO(15), \ + /* PF10 */ ADC3_GPIO(8) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL /* PA0 */ ADC1_CH(0) +#define ADC_SCALED_VDD_3V3_SENSORS2_CHANNEL /* PA4 */ ADC1_CH(4) +#define ADC_SCALED_VDD_3V3_SENSORS3_CHANNEL /* PB0 */ ADC1_CH(8) +#define ADC_SCALED_V5_CHANNEL /* PB1 */ ADC1_CH(9) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL /* PC2 */ ADC1_CH(12) +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC3 */ ADC1_CH(13) + +#define ADC_HW_VER_SENSE_CHANNEL /* PF4 */ ADC3_CH(14) +#define ADC_HW_REV_SENSE_CHANNEL /* PF5 */ ADC3_CH(15) + + +#define ADC_CHANNELS \ + ((1 << ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS2_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS3_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL) | \ + (1 << ADC_BATTERY_VOLTAGE_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 + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ + +#define BOARD_HAS_HW_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 /* PF5 */ ADC3_GPIO(15) +#define GPIO_HW_VER_SENSE /* PF4 */ ADC3_GPIO(14) +#define HW_INFO_INIT {'V','5','X','x', 'x',0} +#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ +#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ + +/* 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)) + +/* PC12 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 + */ +#define GPIO_nARMED_INIT /* PC12 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN12) +#define GPIO_nARMED /* PC12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN12) + +#if !defined(TRACE_PINS) +# 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 8 + +/* Define Battery 1 g Divider and A per V. */ +#define BOARD_BATTERY_V_DIV (13.653333333f) +#define BOARD_BATTERY_A_PER_V (36.367515152f) + +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +#define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN9) + +#define GPIO_VDD_3V5_LTE_nEN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) +#define GPIO_VDD_3V5_LTE_nOC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) +#define GPIO_VDD_5V_HIPOWER_nEN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) +#define GPIO_VDD_5V_HIPOWER_nOC /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_VDD_3V3_SENSORS4_EN /* PG8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN8) + + +/* Spare GPIO */ + +#define GPIO_PH11 /* PH11 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTH|GPIO_PIN11) + + +/* NFC GPIO */ + +#define GPIO_NFC_GPIO /* PH3 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTH|GPIO_PIN3) + + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_3V5_LTE_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V5_LTE_nEN, on_true) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, on_true) +#define VDD_3V3_SENSORS4_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 14 /* Timer 14 */ +#define TONE_ALARM_CHANNEL 1 /* PF9 GPIO_TIM14_CH1OUT_2 */ + +#define GPIO_BUZZER_1 /* PF9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN9) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM14_CH1OUT_2 + +/* 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) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +#define HRT_PPM_CHANNEL /* T8C1 */ 1 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN /* PI5 T8C1 */ GPIO_TIM8_CH1IN_2 + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS5" +#define RC_SERIAL_SINGLEWIRE + +/* Input Capture Channels. */ +#define INPUT_CAP1_TIMER 5 +#define INPUT_CAP1_CHANNEL /* T5C4 */ 4 +#define GPIO_INPUT_CAP1 /* PI0 */ GPIO_TIM5_CH4IN + +/* 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 + +/* 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 GPIO_nSAFETY_SWITCH_LED_OUT_INIT /* PD10 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN10) +#define GPIO_nSAFETY_SWITCH_LED_OUT /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_LED_SAFETY GPIO_nSAFETY_SWITCH_LED_OUT +#define GPIO_SAFETY_SWITCH_IN /* PH4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTH|GPIO_PIN4) +/* 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 */ + +/* Power switch controls ******************************************************/ + +/* + * FMUv5X has a separate RC_IN + * + * GPIO PPM_IN on PI5 T8CH1 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO PPM_IN as an output + */ + +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN5) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +#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_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_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)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) +#define BOARD_ADC_BRICK_VALID (1) //(px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) + +// #define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_3V5_LTE_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + +/* 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_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_VDD_3V5_LTE_nEN, \ + GPIO_VDD_3V5_LTE_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SENSORS4_EN, \ + GPIO_PH11, \ + GPIO_NFC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_PG6, \ + GPIO_nARMED_INIT \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 5 + + +#define PX4_I2C_BUS_MTD 4,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 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/sky-drones/smartap-airlink/src/can.c b/boards/sky-drones/smartap-airlink/src/can.c new file mode 100644 index 000000000000..1a2775d69899 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/can.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32F7_CAN1) && defined(CONFIG_STM32F7_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32F7_CAN2 +#endif + +#ifdef CONFIG_STM32F7_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/sky-drones/smartap-airlink/src/i2c.cpp b/boards/sky-drones/smartap-airlink/src/i2c.cpp new file mode 100644 index 000000000000..eb200ca93204 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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), + initI2CBusExternal(2), + initI2CBusExternal(3), + initI2CBusInternal(4), +}; diff --git a/boards/sky-drones/smartap-airlink/src/init.cpp b/boards/sky-drones/smartap-airlink/src/init.cpp new file mode 100644 index 000000000000..f3ab4a92c4e8 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/init.cpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * 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 init.c + * + * PX4FMU-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 +extern "C" { +#include +} +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * 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 */ + + VDD_3V5_LTE_EN(false); + VDD_5V_HIPOWER_EN(false); + board_control_spi_sensors_power(false, 0xffff); + VDD_3V3_SENSORS4_EN(false); + + /* 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); + VDD_3V5_LTE_EN(true); + VDD_5V_HIPOWER_EN(true); + VDD_3V3_SENSORS4_EN(true); + +} + +/************************************************************************************ + * 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))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +extern "C" __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 */ + VDD_3V5_LTE_EN(true); + VDD_5V_HIPOWER_EN(true); + VDD_3V3_SENSORS4_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + + 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"); + } + + 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); + } + + // Power down the heater + px4_arch_configgpio(GPIO_HEATER_OUTPUT); + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/sky-drones/smartap-airlink/src/led.c b/boards/sky-drones/smartap-airlink/src/led.c new file mode 100644 index 000000000000..fe722c687305 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/led.c @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU 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 + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#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 + GPIO_nSAFETY_SWITCH_LED_OUT_INIT, // Indexed by LED_SAFETY (defaulted to an input) + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +#endif + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + g_ledmap[2] = GPIO_nSAFETY_SWITCH_LED_OUT; + } + + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + 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))); +} + +#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/sky-drones/smartap-airlink/src/manifest.c b/boards/sky-drones/smartap-airlink/src/manifest.c new file mode 100644 index 000000000000..2f7deac66766 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/manifest.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_v0501[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + + +static px4_hw_mft_list_entry_t mft_lists[] = { + {0x0501, hw_mft_list_v0501, arraySize(hw_mft_list_v0501)}, +}; + + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/sky-drones/smartap-airlink/src/mtd.cpp b/boards/sky-drones/smartap-airlink/src/mtd.cpp new file mode 100644 index 000000000000..64006c84e170 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/mtd.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 +// KiB BS nB +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; +static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(3, 0x51) +}; + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi5, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_entry_t base_eeprom = { + .device = &i2c3, + .npart = 2, + .partd = { + { + .type = MTD_MFT, + .path = "/fs/mtd_mft", + .nblocks = 248 + }, + { + .type = MTD_NET, + .path = "/fs/mtd_net", + .nblocks = 8 // 256 = 32 * 8 + + } + }, +}; + + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 2, + .entries = { + &fmum_fram, + &base_eeprom, + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/sky-drones/smartap-airlink/src/sdio.c b/boards/sky-drones/smartap-airlink/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/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/sky-drones/smartap-airlink/src/spi.cpp b/boards/sky-drones/smartap-airlink/src/spi.cpp new file mode 100644 index 000000000000..a0e3e32aed9a --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/spi.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // initSPIDevice(DRV_MAG_DEVTYPE_BMM150, SPI::CS{GPIO::PortH, GPIO::Pin15}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/sky-drones/smartap-airlink/src/timer_config.cpp b/boards/sky-drones/smartap-airlink/src/timer_config.cpp new file mode 100644 index 000000000000..52cce68b944b --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/timer_config.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 + +/* Timer allocation + * + * TIM1_CH4 T FMU_CH1 + * TIM1_CH3 T FMU_CH2 + * TIM1_CH2 T FMU_CH3 + * TIM1_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * + * TIM12_CH1 T FMU_CH7 + * TIM12_CH2 T FMU_CH8 + * + * TIM5_CH4 T FMU_CAP1 < Capture + * TIM5_CH3 T SPI2_DRDY2_ISM330_INT2 < Capture or GPIO INT + * TIM5_CH1 T SPIX_SYNC > Pulse or GPIO strobe + * TIM2_CH3 T HEATER > PWM OUT or GPIO + * + * TIM14_CH1 T BUZZER_1 - Driven by other driver + * TIM8_CH1_IN T FMU_PPM_INPUT - Sampled byt HRT by other driver + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index2, DMA::Stream5, DMA::Channel6}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}), + initIOTimer(Timer::Timer12), + initIOTimer(Timer::Timer5), + initIOTimer(Timer::Timer2), +}; + +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::PortA, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/sky-drones/smartap-airlink/src/usb.c b/boards/sky-drones/smartap-airlink/src/usb.c new file mode 100644 index 000000000000..20d223245cc9 --- /dev/null +++ b/boards/sky-drones/smartap-airlink/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#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 PX4FMU 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_STM32F7_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/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake deleted file mode 100644 index 5de213e2b742..000000000000 --- a/boards/spracing/h7extreme/default.cmake +++ /dev/null @@ -1,121 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR spracing - MODEL h7extreme - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - SERIAL_PORTS -# GPS1:/dev/ttyS0 -# RC:/dev/ttyS1 -# TEL2:/dev/ttyS2 -# TEL4:/dev/ttyS3 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - #imu/analog_devices/adis16448 - #imu/adis16477 - #imu/adis16497 - #imu/bmi088 - imu/invensense/mpu6000 - imu/invensense/icm20602 - #imu/mpu9250 - #irlock - lights # all available light drivers - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - osd - #pca9685 - #power_monitor/ina226 - #protocol_splitter -# pwm_input - Need to create arch/stm32 arch/stm32h7 arch/kinetis and reloacate -# all arch dependant code there - pwm_out_sim - pwm_out - #roboclaw - rc_input - telemetry # all available telemetry drivers - #test_ppm - tone_alarm -# uavcan - No H7 or FD can support in UAVCAN yet - MODULES - #airspeed_selector - attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - #ekf2 - events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - vmount - #vtol_att_control - SYSTEMCMDS - #bl_update - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - #mft - mixer - #motor_ramp - motor_test - #mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - #shutdown - #tests # tests and test runner - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - EXAMPLES - #bottle_drop # OBC challenge - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - ) diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board new file mode 100644 index 000000000000..d1463c40f66a --- /dev/null +++ b/boards/spracing/h7extreme/default.px4board @@ -0,0 +1,67 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=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 +CONFIG_DRIVERS_OSD=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_LAND_DETECTOR=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_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_SENSORS_VEHICLE_AIRSPEED is not set +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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 +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/spracing/h7extreme/init/rc.board_defaults b/boards/spracing/h7extreme/init/rc.board_defaults index 1d809acc3e29..e154e34980bc 100644 --- a/boards/spracing/h7extreme/init/rc.board_defaults +++ b/boards/spracing/h7extreme/init/rc.board_defaults @@ -3,6 +3,9 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.9 +param set-default BAT1_A_PER_V 17 + # system_power unavailable param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/spracing/h7extreme/init/rc.board_mavlink b/boards/spracing/h7extreme/init/rc.board_mavlink deleted file mode 100644 index 43c856670763..000000000000 --- a/boards/spracing/h7extreme/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# SP Racing H7 EXTREME specific specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/spracing/h7extreme/init/rc.board_sensors b/boards/spracing/h7extreme/init/rc.board_sensors index c1277c07dff0..15ceb4bfcaed 100644 --- a/boards/spracing/h7extreme/init/rc.board_sensors +++ b/boards/spracing/h7extreme/init/rc.board_sensors @@ -1,17 +1,23 @@ #!/bin/sh # -# SP Racing H7 EXTREME specific board sensors init +# board specific sensors init #------------------------------------------------------------------------------ -board_adc start -# Internal SPI bus ICM-20602 -#mpu6000 -s -b 2 -R 11 -T 20602 start # SPI 2 -#mpu6000 -s -b 3 -R 10 -T 20602 start # SPI 3 -icm20602 -s -b 2 -R 5 start # SPI 2 -icm20602 -s -b 3 -R 4 start # SPI 3 +board_adc start -# Internal I2C bus -bmp388 -I start +# Internal SPI bus ICM-20602 or ICM-42688-P +# SPI2 +if ! icm20602 -s -b 2 -R 5 start +then + icm42688p -s -b 2 -R 5 start +fi +# SPI3 +if ! icm20602 -s -b 3 -R 4 start +then + icm42688p -s -b 3 -R 4 start +fi +# Internal I2C bus +bmp388 -I start diff --git a/boards/spracing/h7extreme/nuttx-config/nsh/defconfig b/boards/spracing/h7extreme/nuttx-config/nsh/defconfig index a545d288684e..cdcdf8f068bf 100644 --- a/boards/spracing/h7extreme/nuttx-config/nsh/defconfig +++ b/boards/spracing/h7extreme/nuttx-config/nsh/defconfig @@ -19,11 +19,12 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/spracing/h7extreme/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" @@ -38,6 +39,7 @@ CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_ITCM=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y CONFIG_BOARD_LOOPSPERMSEC=95150 @@ -45,6 +47,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x004b CONFIG_CDCACM_PRODUCTSTR="PX4 SP RACING H7 EXTREME" CONFIG_CDCACM_RXBUFSIZE=600 @@ -71,6 +74,7 @@ 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 @@ -83,24 +87,18 @@ CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y CONFIG_MMCSD_SDIO=y CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -110,6 +108,7 @@ 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 @@ -118,6 +117,8 @@ CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAM_SIZE=294912 CONFIG_RAM_START=0x30000000 CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -131,8 +132,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y CONFIG_SIG_SIGALRM_ACTION=y @@ -154,6 +154,7 @@ CONFIG_STM32H7_DMA2=y CONFIG_STM32H7_DMACAPABLE=y CONFIG_STM32H7_DTCMEXCLUDE=y CONFIG_STM32H7_DTCM_PROCFS=y +CONFIG_STM32H7_HSI48=y CONFIG_STM32H7_I2C1=y CONFIG_STM32H7_I2C_DYNTIMEO=y CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 @@ -176,7 +177,6 @@ CONFIG_STM32H7_SPI4=y CONFIG_STM32H7_SPI4_DMA=y CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_UART4=y @@ -194,7 +194,6 @@ CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 CONFIG_TTY_SIGINT=y -CONFIG_TTY_SIGSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 diff --git a/boards/spracing/h7extreme/nuttx-config/scripts/script.ld b/boards/spracing/h7extreme/nuttx-config/scripts/script.ld index c3690768bee1..263688c0b764 100644 --- a/boards/spracing/h7extreme/nuttx-config/scripts/script.ld +++ b/boards/spracing/h7extreme/nuttx-config/scripts/script.ld @@ -142,8 +142,8 @@ SECTIONS *modules__mc_att_control.a:*(.text* .rodata*) *modules__mc_rate_control.a:*(.text* .rodata*) - *modules__rc_update.a:*(.text* .rodata*) *drivers__imu__invensense__icm20602.a:*(.text* .rodata*) + *drivers__imu__invensense__icm42688p.a:*(.text* .rodata*) *drivers__device.a:*(.text* .rodata*) *drivers__pwm_out.a:*(.text* .rodata*) *vehicle_angular_velocity.a:*(.text* .rodata*) @@ -163,6 +163,7 @@ SECTIONS *modules__flight_mode_manager.a:*(.text* .rodata*) *modules__mc_pos_control.a:*(.text* .rodata*) *modules__mc_hover_thrust_estimator.a:*(.text* .rodata*) + *modules__rc_update.a:*(.text* .rodata*) *modules__sensors.a:*(.text* .rodata*) *libc.a:*(.text* .rodata*) *modules__commander.a:*(.text* .rodata*) @@ -238,7 +239,12 @@ SECTIONS *(.gnu.linkonce.d.*) CONSTRUCTORS _edata = ABSOLUTE(.); - } > sram AT > qspi + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > sram AT > qspi = 0xffff .bss : { _sbss = ABSOLUTE(.); diff --git a/boards/spracing/h7extreme/src/board_config.h b/boards/spracing/h7extreme/src/board_config.h index 1fd2a0c2b601..9c1ea8e4310b 100644 --- a/boards/spracing/h7extreme/src/board_config.h +++ b/boards/spracing/h7extreme/src/board_config.h @@ -88,15 +88,9 @@ #define ADC_CHANNELS (1 << 4) | (1 << 10) | (1 << 11) -/* Define Battery 1 Voltage Divider and A per V - */ -#define BOARD_BATTERY1_V_DIV (10.9f) -#define BOARD_BATTERY1_A_PER_V (17.f) - /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define DIRECT_INPUT_TIMER_CHANNELS 8 /* Tone alarm output */ #define GPIO_TONE_ALARM_IDLE /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN5) @@ -122,7 +116,6 @@ #define SDIO_SLOTNO 0 /* Only one slot */ #define SDIO_MINOR 0 -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 @@ -140,7 +133,6 @@ #define BOARD_NUM_IO_TIMERS 3 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {1, 2, 3, 0, 4, 5, 6, 7}; __BEGIN_DECLS diff --git a/boards/spracing/h7extreme/src/flash_w25q128.c b/boards/spracing/h7extreme/src/flash_w25q128.c index 9f943d28544b..3a3e2cee1fcc 100644 --- a/boards/spracing/h7extreme/src/flash_w25q128.c +++ b/boards/spracing/h7extreme/src/flash_w25q128.c @@ -41,6 +41,8 @@ #include "board_config.h" #include "qspi.h" #include "arm_internal.h" +#include +#include /************************************************************************************ @@ -212,7 +214,7 @@ __ramfunc__ int n25qxxx_command(FAR struct qspi_dev_s *qspi, uint8_t cmd) { struct qspi_cmdinfo_s cmdinfo; - finfo("CMD: %02x\n", cmd); + finfo("CMD: %02" PRIx8 "\n", cmd); cmdinfo.flags = 0; cmdinfo.addrlen = 0; @@ -246,7 +248,7 @@ __ramfunc__ int n25qxxx_command_read(FAR struct qspi_dev_s *qspi, uint8_t cmd, { struct qspi_cmdinfo_s cmdinfo; - finfo("CMD: %02x buflen: %lu\n", cmd, (unsigned long)buflen); + finfo("CMD: %02" PRIx8 " buflen: %zu\n", cmd, buflen); cmdinfo.flags = QSPICMD_READDATA; cmdinfo.addrlen = 0; @@ -303,7 +305,7 @@ __ramfunc__ int n25qxxx_write_page(struct n25qxxx_dev_s *priv, FAR const uint8_t int ret = OK; unsigned int i; - finfo("address: %08lx buflen: %u\n", (unsigned long)address, (unsigned)buflen); + finfo("address: %08jx buflen: %zu\n", (intmax_t)address, buflen); pagesize = (1 << priv->pageshift); @@ -378,7 +380,7 @@ __ramfunc__ int n25qxxx_write_one_page(struct n25qxxx_dev_s *priv, struct qspi_m n25qxxx_write_disable(priv); if (ret < 0) { - ferr("ERROR: QSPI_MEMORY failed writing address=%06x\n", + ferr("ERROR: QSPI_MEMORY failed writing address=%06" PRIx32 "\n", meminfo->addr); } @@ -394,14 +396,14 @@ __ramfunc__ int n25qxxx_erase_sector(struct n25qxxx_dev_s *priv, off_t sector) off_t address; uint8_t status; - finfo("sector: %08lx\n", (unsigned long)sector); + finfo("sector: %08jx\n", (intmax_t) sector); /* Check that the flash is ready and unprotected */ status = n25qxxx_read_status(priv); if ((status & STATUS_BUSY_MASK) != STATUS_READY) { - ferr("ERROR: Flash busy: %02x", status); + ferr("ERROR: Flash busy: %02" PRIx8, status); return -EBUSY; } @@ -411,7 +413,7 @@ __ramfunc__ int n25qxxx_erase_sector(struct n25qxxx_dev_s *priv, off_t sector) if ((status & (STATUS_BP3_MASK | STATUS_BP_MASK)) != 0 && n25qxxx_isprotected(priv, status, address)) { - ferr("ERROR: Flash protected: %02x", status); + ferr("ERROR: Flash protected: %02" PRIx8, status); return -EACCES; } @@ -488,7 +490,7 @@ __ramfunc__ int n25qxxx_command_address(FAR struct qspi_dev_s *qspi, uint8_t cm { struct qspi_cmdinfo_s cmdinfo; - finfo("CMD: %02x Address: %04lx addrlen=%d\n", cmd, (unsigned long)addr, addrlen); + finfo("CMD: %02" PRIx8 " Address: %04jx addrlen=%" PRIx8 "\n", cmd, (intmax_t) addr, addrlen); cmdinfo.flags = QSPICMD_ADDRESS; cmdinfo.addrlen = addrlen; diff --git a/boards/spracing/h7extreme/src/init.c b/boards/spracing/h7extreme/src/init.c index 6a4004c6caa4..b241fb62865b 100644 --- a/boards/spracing/h7extreme/src/init.c +++ b/boards/spracing/h7extreme/src/init.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -204,26 +205,12 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); } -#if 0 // serial DMA is not yet implemented in NuttX for stm32h7 - /* set up the serial DMA polling */ +#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; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + 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); @@ -238,7 +225,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); - return ret; } #endif @@ -286,7 +272,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (result != OK) { syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); led_on(LED_AMBER); - return -ENODEV; } #endif diff --git a/boards/spracing/h7extreme/src/qspi.c b/boards/spracing/h7extreme/src/qspi.c index 26d1d7dc4b2c..5e8df9e81622 100644 --- a/boards/spracing/h7extreme/src/qspi.c +++ b/boards/spracing/h7extreme/src/qspi.c @@ -457,7 +457,7 @@ inline uint32_t qspi_getreg(struct stm32h7_qspidev_s *priv, unsigned int offset) #ifdef CONFIG_STM32H7_QSPI_REGDEBUG if (qspi_checkreg(priv, false, value, address)) { - spiinfo("%08x->%08x\n", address, value); + spiinfo("%08" PRIx32 "->%08" PRIx32 "\n", address, value); } #endif @@ -481,7 +481,7 @@ static inline void qspi_putreg(struct stm32h7_qspidev_s *priv, uint32_t value, #ifdef CONFIG_STM32H7_QSPI_REGDEBUG if (qspi_checkreg(priv, true, value, address)) { - spiinfo("%08x<-%08x\n", address, value); + spiinfo("%08" PRIx32 "<-%08" PRIx32 "\n", address, value); } #endif @@ -516,11 +516,11 @@ QUADSPI_RAMFUNC void qspi_dumpregs(struct stm32h7_qspidev_s *priv, const char *m */ regval = getreg32(priv->base + STM32_QUADSPI_CR_OFFSET); /* Control Register */ - spiinfo("CR:%08x\n", regval); + spiinfo("CR:%08" PRIx32 "\n", regval); spiinfo(" EN:%1d ABORT:%1d DMAEN:%1d TCEN:%1d SSHIFT:%1d\n" " FTHRES: %d\n" " TEIE:%1d TCIE:%1d FTIE:%1d SMIE:%1d TOIE:%1d APMS:%1d PMM:%1d\n" - " PRESCALER: %d\n", + " PRESCALER: %" PRId32 "\n", (regval & QSPI_CR_EN) ? 1 : 0, (regval & QSPI_CR_ABORT) ? 1 : 0, (regval & QSPI_CR_DMAEN) ? 1 : 0, @@ -537,16 +537,16 @@ QUADSPI_RAMFUNC void qspi_dumpregs(struct stm32h7_qspidev_s *priv, const char *m (regval & QSPI_CR_PRESCALER_MASK) >> QSPI_CR_PRESCALER_SHIFT); regval = getreg32(priv->base + STM32_QUADSPI_DCR_OFFSET); /* Device Configuration Register */ - spiinfo("DCR:%08x\n", regval); - spiinfo(" CKMODE:%1d CSHT:%d FSIZE:%d\n", + spiinfo("DCR:%08" PRIx32 "\n", regval); + spiinfo(" CKMODE:%1d CSHT:%" PRId32 " FSIZE:%" PRId32 "\n", (regval & QSPI_DCR_CKMODE) ? 1 : 0, (regval & QSPI_DCR_CSHT_MASK) >> QSPI_DCR_CSHT_SHIFT, (regval & QSPI_DCR_FSIZE_MASK) >> QSPI_DCR_FSIZE_SHIFT); regval = getreg32(priv->base + STM32_QUADSPI_CCR_OFFSET); /* Communication Configuration Register */ - spiinfo("CCR:%08x\n", regval); - spiinfo(" INST:%02x IMODE:%d ADMODE:%d ADSIZE:%d ABMODE:%d\n" - " ABSIZE:%d DCYC:%d DMODE:%d FMODE:%d\n" + spiinfo("CCR:%08" PRIx32 "\n", regval); + spiinfo(" INST:%02" PRId32 " IMODE:%" PRId32 " ADMODE:%" PRId32 " ADSIZE:%" PRId32 " ABMODE:%" PRId32 "\n" + " ABSIZE:%" PRId32 " DCYC:%" PRId32 " DMODE:%" PRId32 " FMODE:%" PRId32 "\n" " SIOO:%1d DDRM:%1d\n", (regval & QSPI_CCR_INSTRUCTION_MASK) >> QSPI_CCR_INSTRUCTION_SHIFT, (regval & QSPI_CCR_IMODE_MASK) >> QSPI_CCR_IMODE_SHIFT, @@ -561,8 +561,8 @@ QUADSPI_RAMFUNC void qspi_dumpregs(struct stm32h7_qspidev_s *priv, const char *m (regval & QSPI_CCR_DDRM) ? 1 : 0); regval = getreg32(priv->base + STM32_QUADSPI_SR_OFFSET); /* Status Register */ - spiinfo("SR:%08x\n", regval); - spiinfo(" TEF:%1d TCF:%1d FTF:%1d SMF:%1d TOF:%1d BUSY:%1d FLEVEL:%d\n", + spiinfo("SR:%08" PRIx32 "\n", regval); + spiinfo(" TEF:%1d TCF:%1d FTF:%1d SMF:%1d TOF:%1d BUSY:%1d FLEVEL:%" PRId32 "\n", (regval & QSPI_SR_TEF) ? 1 : 0, (regval & QSPI_SR_TCF) ? 1 : 0, (regval & QSPI_SR_FTF) ? 1 : 0, @@ -572,17 +572,17 @@ QUADSPI_RAMFUNC void qspi_dumpregs(struct stm32h7_qspidev_s *priv, const char *m (regval & QSPI_SR_FLEVEL_MASK) >> QSPI_SR_FLEVEL_SHIFT); #else - spiinfo(" CR:%08x DCR:%08x CCR:%08x SR:%08x\n", + spiinfo(" CR:%08" PRIx32 " DCR:%08" PRIx32 " CCR:%08" PRIx32 " SR:%08" PRIx32 "\n", getreg32(priv->base + STM32_QUADSPI_CR_OFFSET), /* Control Register */ getreg32(priv->base + STM32_QUADSPI_DCR_OFFSET), /* Device Configuration Register */ getreg32(priv->base + STM32_QUADSPI_CCR_OFFSET), /* Communication Configuration Register */ getreg32(priv->base + STM32_QUADSPI_SR_OFFSET)); /* Status Register */ - spiinfo(" DLR:%08x ABR:%08x PSMKR:%08x PSMAR:%08x\n", + spiinfo(" DLR:%08" PRIx32 " ABR:%08" PRIx32 " PSMKR:%08" PRIx32 " PSMAR:%08" PRIx32 "\n", getreg32(priv->base + STM32_QUADSPI_DLR_OFFSET), /* Data Length Register */ getreg32(priv->base + STM32_QUADSPI_ABR_OFFSET), /* Alternate Bytes Register */ getreg32(priv->base + STM32_QUADSPI_PSMKR_OFFSET), /* Polling Status mask Register */ getreg32(priv->base + STM32_QUADSPI_PSMAR_OFFSET)); /* Polling Status match Register */ - spiinfo(" PIR:%08x LPTR:%08x\n", + spiinfo(" PIR:%08" PRIx32 " LPTR:%08" PRIx32 "\n", getreg32(priv->base + STM32_QUADSPI_PIR_OFFSET), /* Polling Interval Register */ getreg32(priv->base + STM32_QUADSPI_LPTR_OFFSET)); /* Low-Power Timeout Register */ (void)regval; @@ -599,62 +599,62 @@ QUADSPI_RAMFUNC void qspi_dumpgpioconfig(const char *msg) /* Port B */ regval = getreg32(STM32_GPIOB_MODER); - spiinfo("B_MODER:%08x\n", regval); + spiinfo("B_MODER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOB_OTYPER); - spiinfo("B_OTYPER:%08x\n", regval); + spiinfo("B_OTYPER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOB_OSPEED); - spiinfo("B_OSPEED:%08x\n", regval); + spiinfo("B_OSPEED:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOB_PUPDR); - spiinfo("B_PUPDR:%08x\n", regval); + spiinfo("B_PUPDR:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOB_AFRL); - spiinfo("B_AFRL:%08x\n", regval); + spiinfo("B_AFRL:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOB_AFRH); - spiinfo("B_AFRH:%08x\n", regval); + spiinfo("B_AFRH:%" PRIx32 "\n", regval); /* Port D */ regval = getreg32(STM32_GPIOD_MODER); - spiinfo("D_MODER:%08x\n", regval); + spiinfo("D_MODER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOD_OTYPER); - spiinfo("D_OTYPER:%08x\n", regval); + spiinfo("D_OTYPER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOD_OSPEED); - spiinfo("D_OSPEED:%08x\n", regval); + spiinfo("D_OSPEED:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOD_PUPDR); - spiinfo("D_PUPDR:%08x\n", regval); + spiinfo("D_PUPDR:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOD_AFRL); - spiinfo("D_AFRL:%08x\n", regval); + spiinfo("D_AFRL:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOD_AFRH); - spiinfo("D_AFRH:%08x\n", regval); + spiinfo("D_AFRH:%" PRIx32 "\n", regval); /* Port E */ regval = getreg32(STM32_GPIOE_MODER); - spiinfo("E_MODER:%08x\n", regval); + spiinfo("E_MODER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOE_OTYPER); - spiinfo("E_OTYPER:%08x\n", regval); + spiinfo("E_OTYPER:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOE_OSPEED); - spiinfo("E_OSPEED:%08x\n", regval); + spiinfo("E_OSPEED:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOE_PUPDR); - spiinfo("E_PUPDR:%08x\n", regval); + spiinfo("E_PUPDR:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOE_AFRL); - spiinfo("E_AFRL:%08x\n", regval); + spiinfo("E_AFRL:%" PRIx32 "\n", regval); regval = getreg32(STM32_GPIOE_AFRH); - spiinfo("E_AFRH:%08x\n", regval); + spiinfo("E_AFRH:%" PRIx32 "\n", regval); } #endif @@ -766,18 +766,18 @@ QUADSPI_RAMFUNC int qspi_setupxctnfromcmd(struct qspi_xctnspec_s *xctn, #ifdef CONFIG_DEBUG_SPI_INFO spiinfo("Transfer:\n"); - spiinfo(" flags: %02x\n", cmdinfo->flags); - spiinfo(" cmd: %04x\n", cmdinfo->cmd); + spiinfo(" flags: %02" PRIx8 "\n", cmdinfo->flags); + spiinfo(" cmd: %04" PRIx16 "\n", cmdinfo->cmd); if (QSPICMD_ISADDRESS(cmdinfo->flags)) { - spiinfo(" address/length: %08lx/%d\n", - (unsigned long)cmdinfo->addr, cmdinfo->addrlen); + spiinfo(" address/length: %08" PRIx32 "/%" PRId8 "\n", + cmdinfo->addr, cmdinfo->addrlen); } if (QSPICMD_ISDATA(cmdinfo->flags)) { spiinfo(" %s Data:\n", QSPICMD_ISWRITE(cmdinfo->flags) ? "Write" : "Read"); - spiinfo(" buffer/length: %p/%d\n", + spiinfo(" buffer/length: %p/%" PRId16 "\n", cmdinfo->buffer, cmdinfo->buflen); } @@ -895,13 +895,13 @@ QUADSPI_RAMFUNC int qspi_setupxctnfrommem(struct qspi_xctnspec_s *xctn, #ifdef CONFIG_DEBUG_SPI_INFO spiinfo("Transfer:\n"); - spiinfo(" flags: %02x\n", meminfo->flags); - spiinfo(" cmd: %04x\n", meminfo->cmd); - spiinfo(" address/length: %08lx/%d\n", - (unsigned long)meminfo->addr, meminfo->addrlen); + spiinfo(" flags: %02" PRIx32 "\n", meminfo->flags); + spiinfo(" cmd: %04" PRIx16 "\n", meminfo->cmd); + spiinfo(" address/length: %08" PRIx32 "/%" PRId8 "\n", + meminfo->addr, meminfo->addrlen); spiinfo(" %s Data:\n", QSPIMEM_ISWRITE(meminfo->flags) ? "Write" : "Read"); - spiinfo(" buffer/length: %p/%d\n", meminfo->buffer, meminfo->buflen); + spiinfo(" buffer/length: %p/%" PRId32 "\n", meminfo->buffer, meminfo->buflen); #endif DEBUGASSERT(meminfo->cmd < 256); @@ -1789,7 +1789,7 @@ QUADSPI_RAMFUNC uint32_t qspi_setfrequency(struct qspi_dev_s *dev, return 0; } - spiinfo("frequency=%d\n", frequency); + spiinfo("frequency=%" PRId32 "\n", frequency); DEBUGASSERT(priv); /* Wait till BUSY flag reset */ @@ -1840,14 +1840,14 @@ QUADSPI_RAMFUNC uint32_t qspi_setfrequency(struct qspi_dev_s *dev, /* Calculate the new actual frequency */ actual = QSPI_CLK_FREQUENCY / prescaler; - spiinfo("prescaler=%d actual=%d\n", prescaler, actual); + spiinfo("prescaler=%" PRId32 " actual=%" PRId32 "\n", prescaler, actual); /* Save the frequency setting */ priv->frequency = frequency; priv->actual = actual; - spiinfo("Frequency %d->%d\n", frequency, actual); + spiinfo("Frequency %" PRId32 "->%" PRId32 "\n", frequency, actual); return actual; } @@ -1916,7 +1916,7 @@ QUADSPI_RAMFUNC void qspi_setmode(struct qspi_dev_s *dev, enum qspi_mode_e mode) } qspi_putreg(priv, regval, STM32_QUADSPI_DCR_OFFSET); - spiinfo("DCR=%08x\n", regval); + spiinfo("DCR=%" PRIx32 "\n", regval); /* Save the mode so that subsequent re-configurations will be faster */ @@ -2587,7 +2587,7 @@ struct qspi_dev_s *stm32h7_qspi_initialize(int intf) ret = irq_attach(priv->irq, priv->handler, NULL); if (ret < 0) { - spierr("ERROR: Failed to attach irq %d\n", priv->irq); + spierr("ERROR: Failed to attach irq %" PRId8 "\n", priv->irq); goto errout_with_dmadog; } diff --git a/boards/spracing/h7extreme/src/qspi.h b/boards/spracing/h7extreme/src/qspi.h index 87c5af921289..f93573ddb716 100644 --- a/boards/spracing/h7extreme/src/qspi.h +++ b/boards/spracing/h7extreme/src/qspi.h @@ -43,6 +43,7 @@ #include #include +#include #include #include diff --git a/boards/spracing/h7extreme/src/rcc.c b/boards/spracing/h7extreme/src/rcc.c index 9da231548cee..9fc05848163a 100644 --- a/boards/spracing/h7extreme/src/rcc.c +++ b/boards/spracing/h7extreme/src/rcc.c @@ -194,7 +194,6 @@ __ramfunc__ void stm32_board_clockconfig(void) #endif -#define CONFIG_STM32H7_HSI48 #ifdef CONFIG_STM32H7_HSI48 /* Enable HSI48 */ diff --git a/boards/spracing/h7extreme/src/timer_config.cpp b/boards/spracing/h7extreme/src/timer_config.cpp index 0ccf044d3bae..d7c83bbd9aac 100644 --- a/boards/spracing/h7extreme/src/timer_config.cpp +++ b/boards/spracing/h7extreme/src/timer_config.cpp @@ -48,10 +48,9 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortB, GPIO::Pin6}), //Motor 5 initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortB, GPIO::Pin7}), //Motor 6 - /* - initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin6}), //Motor 9 - initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin7}), //Motor 10 - */ + +// initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin6}), //Motor 9 +// initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin7}), //Motor 10 initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortC, GPIO::Pin6}), //Motor 7 initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortC, GPIO::Pin7}), //Motor 8 diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake deleted file mode 100644 index 591b1d17bd11..000000000000 --- a/boards/uvify/core/default.cmake +++ /dev/null @@ -1,100 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR uvify - MODEL core - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL3:/dev/ttyS6 - DRIVERS - adc/board_adc - barometer/ms5611 - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - dshot - gps - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights/rgbled_ncp5623c - magnetometer/bosch/bmm150 - magnetometer/lis3mdl - magnetometer/isentek/ist8310 - optical_flow # all available optical flow drivers - pca9685 - pwm_input - pwm_out_sim - pwm_out - rc_input - tone_alarm - uavcan - MODULES - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - gyro_calibration - gyro_fft - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - sensors - sih - temperature_compensation - vmount - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - serial_test - system_time - top - topic_listener - tune_control - uorb - usb_connected - ver - work_queue - ) diff --git a/boards/uvify/core/default.px4board b/boards/uvify/core/default.px4board new file mode 100644 index 000000000000..e2f5ef42d2ad --- /dev/null +++ b/boards/uvify/core/default.px4board @@ -0,0 +1,91 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685=y +CONFIG_DRIVERS_PWM_INPUT=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=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_CONTROL_ALLOCATOR=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_GIMBAL=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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/uvify/core/bootloader/uvify_core_bootloader.bin b/boards/uvify/core/extras/uvify_core_bootloader.bin similarity index 100% rename from boards/uvify/core/bootloader/uvify_core_bootloader.bin rename to boards/uvify/core/extras/uvify_core_bootloader.bin diff --git a/boards/uvify/core/init/rc.board_defaults b/boards/uvify/core/init/rc.board_defaults index df7abbbbef6b..fedd27a1604e 100644 --- a/boards/uvify/core/init/rc.board_defaults +++ b/boards/uvify/core/init/rc.board_defaults @@ -3,5 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default BAT1_V_DIV 10.14 +param set-default BAT1_A_PER_V 18.18 + # don't probe external I2C param set-default SENS_EXT_I2C_PRB 0 diff --git a/boards/uvify/core/init/rc.board_extras b/boards/uvify/core/init/rc.board_extras index 952a0162af5f..2acb1c13af01 100644 --- a/boards/uvify/core/init/rc.board_extras +++ b/boards/uvify/core/init/rc.board_extras @@ -7,9 +7,6 @@ # IFO if param compare SYS_AUTOSTART 4071 then - # Change rate to 400 Khz for fast barometer - #pwm_out i2c 1 400000 - # IFO has only external i2c barometer. # It does not start EKF2 in the beginning which is strange behaviour. but 3 seconds hack. # We intentionally put this initialization to here for delayed initialization. diff --git a/boards/uvify/core/init/rc.board_mavlink b/boards/uvify/core/init/rc.board_mavlink deleted file mode 100644 index 5b92aba400a8..000000000000 --- a/boards/uvify/core/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# UVify UVF4 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/uvify/core/nuttx-config/nsh/defconfig b/boards/uvify/core/nuttx-config/nsh/defconfig index 565d631ad54e..08921db7e126 100644 --- a/boards/uvify/core/nuttx-config/nsh/defconfig +++ b/boards/uvify/core/nuttx-config/nsh/defconfig @@ -19,12 +19,13 @@ # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set # CONFIG_NSH_DISABLE_SEMICOLON is not set # CONFIG_NSH_DISABLE_TIME is not set # CONFIG_STM32_CCMEXCLUDE is not set CONFIG_ARCH="arm" CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/uvify/core/nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32" @@ -41,6 +42,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU UVify Core" CONFIG_CDCACM_RXBUFSIZE=600 @@ -90,18 +92,13 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_MW=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 @@ -117,7 +114,6 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_ROBUST=y CONFIG_PTHREAD_STACK_MIN=512 CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=262144 CONFIG_RAM_START=0x20000000 CONFIG_RAW_BINARY=y @@ -134,8 +130,7 @@ CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y CONFIG_SERIAL_TERMIOS=y CONFIG_SIG_DEFAULT=y @@ -175,7 +170,7 @@ 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_SPI1_DMA_BUFFER=512 CONFIG_STM32_SPI2=y CONFIG_STM32_SPI_DMA=y CONFIG_STM32_SPI_DMATHRESHOLD=8 diff --git a/boards/uvify/core/src/CMakeLists.txt b/boards/uvify/core/src/CMakeLists.txt index f17f030cf284..8e470f2ead15 100644 --- a/boards/uvify/core/src/CMakeLists.txt +++ b/boards/uvify/core/src/CMakeLists.txt @@ -48,5 +48,4 @@ target_link_libraries(drivers_board nuttx_arch # sdio nuttx_drivers # sdio px4_layer - arch_io_pins ) diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h index de1b4730c5d9..22708b23e29a 100644 --- a/boards/uvify/core/src/board_config.h +++ b/boards/uvify/core/src/board_config.h @@ -83,10 +83,6 @@ #define ADC_5V_RAIL_SENSE 4 #define ADC_RC_RSSI_CHANNEL 11 -/* Define Battery 1 Voltage Divider and A per V. */ -#define BOARD_BATTERY1_V_DIV (10.14f) -#define BOARD_BATTERY1_A_PER_V (18.18f) - /* Power supply control and monitoring GPIOs. */ #define GPIO_VDD_BRICK_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) #define GPIO_VDD_USB_VALID (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) @@ -101,7 +97,6 @@ * PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 6 -#define DIRECT_INPUT_TIMER_CHANNELS 6 /** * USB OTG FS: @@ -145,6 +140,7 @@ /* Heater pins (reserved) */ #define GPIO_HEATER_INPUT (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6) #define GPIO_HEATER_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) /* Power switch controls */ @@ -176,14 +172,12 @@ #define BOARD_ADC_PERIPH_5V_OC (0) #define BOARD_ADC_HIPOWER_5V_OC (0) -#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS /* This board provides a DMA pool and APIs */ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 #define BOARD_HAS_ON_RESET 1 -#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) diff --git a/boards/uvify/core/src/init.c b/boards/uvify/core/src/init.c index 13da4c6ba53a..3418877c6f73 100644 --- a/boards/uvify/core/src/init.c +++ b/boards/uvify/core/src/init.c @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -266,22 +267,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "DMA alloc FAILED\n"); } - // Set up the serial DMA polling. +#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; - struct timespec ts; - - /** - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +#endif // Initial LED state. drv_led_start(); @@ -302,7 +292,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi1) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); led_on(LED_RED); - return -ENODEV; } @@ -318,7 +307,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!spi2) { syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); led_on(LED_RED); - return -ENODEV; } /** @@ -360,9 +348,7 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (!sdio) { led_on(LED_RED); - syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", - CONFIG_NSH_MMCSDSLOTNO); - return -ENODEV; + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); } // Now bind the SDIO interface to the MMC/SD driver. @@ -371,7 +357,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) if (ret != OK) { led_on(LED_RED); syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); - return ret; } // Then let's guess and say that there is a card in the slot. There is no card detect GPIO. diff --git a/cmake/bloaty.cmake b/cmake/bloaty.cmake index 7a0f25b48ef5..b20537adb13e 100644 --- a/cmake/bloaty.cmake +++ b/cmake/bloaty.cmake @@ -71,9 +71,16 @@ if (BLOATY_PROGRAM) USES_TERMINAL ) + # bloaty statically allocated RAM + add_custom_target(bloaty_ram + COMMAND ${BLOATY_PROGRAM} -c ${PX4_SOURCE_DIR}/Tools/bloaty_static_ram.bloaty -d bloaty_static_ram,compileunits --source-filter ^ram$ ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + # bloaty compare with last master build add_custom_target(bloaty_compare_master - COMMAND wget -c -N --no-verbose https://s3.amazonaws.com/px4-travis/Firmware/master/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf -O master.elf + COMMAND wget --continue --no-verbose https://s3.amazonaws.com/px4-travis/Firmware/master/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf -O master.elf COMMAND ${BLOATY_PROGRAM} -d symbols ${BLOATY_OPTS} $ -- master.elf DEPENDS px4 WORKING_DIRECTORY ${PX4_BINARY_DIR} diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 5560fca0621d..1a54456194c1 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -44,7 +44,7 @@ function(px4_add_unit_gtest) px4_parse_function_args( NAME px4_add_unit_gtest ONE_VALUE SRC - MULTI_VALUE LINKLIBS + MULTI_VALUE EXTRA_SRCS COMPILE_FLAGS INCLUDES LINKLIBS REQUIRED SRC ARGN ${ARGN}) @@ -54,11 +54,19 @@ function(px4_add_unit_gtest) set(TESTNAME unit-${TESTNAME}) # build a binary for the unit test - add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC}) + add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC} ${EXTRA_SRCS}) # link the libary to test and gtest target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_main) + if(COMPILE_FLAGS) + target_compile_options(${TESTNAME} PRIVATE ${COMPILE_FLAGS}) + endif() + + if(INCLUDES) + target_include_directories(${TESTNAME} PRIVATE ${INCLUDES}) + endif() + # add the test to the ctest plan add_test(NAME ${TESTNAME} COMMAND ${TESTNAME} @@ -76,7 +84,7 @@ function(px4_add_functional_gtest) px4_parse_function_args( NAME px4_add_functional_gtest ONE_VALUE SRC - MULTI_VALUE LINKLIBS + MULTI_VALUE EXTRA_SRCS COMPILE_FLAGS INCLUDES LINKLIBS REQUIRED SRC ARGN ${ARGN}) @@ -86,7 +94,7 @@ function(px4_add_functional_gtest) set(TESTNAME functional-${TESTNAME}) # build a binary for the unit test - add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC}) + add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC} ${EXTRA_SRCS}) # link the libary to test and gtest target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_functional_main @@ -104,6 +112,14 @@ function(px4_add_functional_gtest) uorb_msgs test_stubs) #put test_stubs last + if(COMPILE_FLAGS) + target_compile_options(${TESTNAME} PRIVATE ${COMPILE_FLAGS}) + endif() + + if(INCLUDES) + target_include_directories(${TESTNAME} PRIVATE ${INCLUDES}) + endif() + # add the test to the ctest plan add_test(NAME ${TESTNAME} # functional tests need to run in a new process for each test, diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake new file mode 100644 index 000000000000..71878e6ba3e4 --- /dev/null +++ b/cmake/kconfig.cmake @@ -0,0 +1,425 @@ +set(BOARD_DEFCONFIG ${PX4_CONFIG_FILE} CACHE FILEPATH "path to defconfig" FORCE) +set(BOARD_CONFIG ${PX4_BINARY_DIR}/boardconfig CACHE FILEPATH "path to config" FORCE) + +execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import menuconfig" RESULT_VARIABLE ret) +if(ret EQUAL "1") + message(FATAL_ERROR "kconfiglib is not installed or not in PATH\n" + "please install using \"pip3 install kconfiglib\"\n") +endif() + +set(MENUCONFIG_PATH ${PYTHON_EXECUTABLE} -m menuconfig CACHE INTERNAL "menuconfig program" FORCE) +set(GUICONFIG_PATH ${PYTHON_EXECUTABLE} -m guiconfig CACHE INTERNAL "guiconfig program" FORCE) +set(DEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m defconfig CACHE INTERNAL "defconfig program" FORCE) +set(SAVEDEFCONFIG_PATH ${PYTHON_EXECUTABLE} -m savedefconfig CACHE INTERNAL "savedefconfig program" FORCE) +set(GENCONFIG_PATH ${PYTHON_EXECUTABLE} -m genconfig CACHE INTERNAL "genconfig program" FORCE) + +set(COMMON_KCONFIG_ENV_SETTINGS + PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} + KCONFIG_CONFIG=${BOARD_CONFIG} + # Set environment variables so that Kconfig can prune Kconfig source + # files for other architectures + PLATFORM=${PX4_PLATFORM} + VENDOR=${PX4_BOARD_VENDOR} + MODEL=${PX4_BOARD_MODEL} + LABEL=${PX4_BOARD_LABEL} + TOOLCHAIN=${CMAKE_TOOLCHAIN_FILE} + ARCHITECTURE=${CMAKE_SYSTEM_PROCESSOR} + ROMFSROOT=${config_romfs_root} +) + +set(config_user_list) + +if(EXISTS ${BOARD_DEFCONFIG}) + + # Depend on BOARD_DEFCONFIG so that we reconfigure on config change + set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG}) + + if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") + # Generate boardconfig from saved defconfig + execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} + ${DEFCONFIG_PATH} ${BOARD_DEFCONFIG} + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + OUTPUT_VARIABLE DUMMY_RESULTS) + else() + # Generate boardconfig from default.px4board and {label}.px4board + execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} + ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/merge_config.py Kconfig ${BOARD_CONFIG} ${PX4_BOARD_DIR}/default.px4board ${BOARD_DEFCONFIG} + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + OUTPUT_VARIABLE DUMMY_RESULTS) + endif() + + # Generate header file for C/C++ preprocessor + execute_process(COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} + ${GENCONFIG_PATH} --header-path ${PX4_BINARY_DIR}/px4_boardconfig.h + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + OUTPUT_VARIABLE DUMMY_RESULTS) + + # parse board config options for cmake + file(STRINGS ${BOARD_CONFIG} ConfigContents) + foreach(NameAndValue ${ConfigContents}) + # Strip leading spaces + string(REGEX REPLACE "^[ ]+" "" NameAndValue ${NameAndValue}) + + # Find variable name + string(REGEX MATCH "^CONFIG[^=]+" Name ${NameAndValue}) + + if(Name) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + + if(Value) + # remove extra quotes + string(REPLACE "\"" "" Value ${Value}) + + # Set the variable + set(${Name} ${Value} CACHE INTERNAL "BOARD DEFCONFIG: ${Name}" FORCE) + endif() + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_BOARD_" Board ${NameAndValue}) + + if(Board) + string(REPLACE "CONFIG_BOARD_" "" ConfigKey ${Name}) + if(Value) + set(${ConfigKey} ${Value}) + message(STATUS "${ConfigKey} ${Value}") + endif() + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_USER[^=]+" Userspace ${NameAndValue}) + + if(Userspace) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + string(REPLACE "CONFIG_USER_" "" module ${Name}) + string(TOLOWER ${module} module) + list(APPEND config_user_list ${module}) + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_DRIVERS[^=]+" Drivers ${NameAndValue}) + + if(Drivers) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + string(REPLACE "CONFIG_DRIVERS_" "" driver ${Name}) + string(TOLOWER ${driver} driver) + + string(REPLACE "_" "/" driver_path ${driver}) + + # Pattern 1 XXX / XXX_XXX + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\1" driver_p1_folder ${driver}) + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\2" driver_p1_subfolder ${driver}) + + # Pattern 2 XXX_XXX / XXXXXX + string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z0-9]+).*$" "\\1" driver_p2_folder ${driver}) + string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z0-9]+).*$" "\\2" driver_p2_subfolder ${driver}) + + # Pattern 3 XXXXXX / XXX_XXX / XXXXXX + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+_[a-z0-9]+)_([a-z]+[a-z0-9]+).*$" "\\1" driver_p3_folder ${driver}) + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+_[a-z0-9]+)_([a-z]+[a-z0-9]+).*$" "\\2" driver_p3_subfolder ${driver}) + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+_[a-z0-9]+)_([a-z]+[a-z0-9]+).*$" "\\3" driver_p3_subsubfolder ${driver}) + + # Pattern 4 XXX_XXX / XXX_XXX_XXX + string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z_0-9]+).*$" "\\1" driver_p4_folder ${driver}) + string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z_0-9]+).*$" "\\2" driver_p4_subfolder ${driver}) + + # Pattern 5 XXXXXX / XXXXXX / XXX_XXX + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\1" driver_p5_folder ${driver}) + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\2" driver_p5_subfolder ${driver}) + string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\3" driver_p5_subsubfolder ${driver}) + + # Trick circumvent PX4 src naming problem with underscores and slashes + if(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver}) + list(APPEND config_module_list drivers/${driver}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_path}) + list(APPEND config_module_list drivers/${driver_path}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p3_folder}/${driver_p3_subfolder}/${driver_p3_subsubfolder}) + list(APPEND config_module_list drivers/${driver_p3_folder}/${driver_p3_subfolder}/${driver_p3_subsubfolder}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p1_folder}/${driver_p1_subfolder}) + list(APPEND config_module_list drivers/${driver_p1_folder}/${driver_p1_subfolder}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p4_folder}/${driver_p4_subfolder}) + list(APPEND config_module_list drivers/${driver_p4_folder}/${driver_p4_subfolder}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p2_folder}/${driver_p2_subfolder}) + list(APPEND config_module_list drivers/${driver_p2_folder}/${driver_p2_subfolder}) + elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p5_folder}/${driver_p5_subfolder}/${driver_p5_subsubfolder}) + list(APPEND config_module_list drivers/${driver_p5_folder}/${driver_p5_subfolder}/${driver_p5_subsubfolder}) + else() + message(FATAL_ERROR "Couldn't find path for ${driver}") + endif() + + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_MODULES[^=]+" Modules ${NameAndValue}) + + if(Modules) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + string(REPLACE "CONFIG_MODULES_" "" module ${Name}) + string(TOLOWER ${module} module) + + list(APPEND config_module_list modules/${module}) + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_SYSTEMCMDS[^=]+" Systemcmds ${NameAndValue}) + + if(Systemcmds) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + string(REPLACE "CONFIG_SYSTEMCMDS_" "" systemcmd ${Name}) + string(TOLOWER ${systemcmd} systemcmd) + + list(APPEND config_module_list systemcmds/${systemcmd}) + endif() + + # Find variable name + string(REGEX MATCH "^CONFIG_EXAMPLES[^=]+" Examples ${NameAndValue}) + + if(Examples) + # Find the value + string(REPLACE "${Name}=" "" Value ${NameAndValue}) + string(REPLACE "CONFIG_EXAMPLES_" "" example ${Name}) + string(TOLOWER ${example} example) + + list(APPEND config_module_list examples/${example}) + endif() + + endforeach() + + # Put every module not in userspace also to kernel list + foreach(modpath ${config_module_list}) + get_filename_component(module ${modpath} NAME) + list(FIND config_user_list ${module} _index) + + if (${_index} EQUAL -1) + list(APPEND config_kernel_list ${modpath}) + endif() + endforeach() + + if(PLATFORM) + # set OS, and append specific platform module path + set(PX4_PLATFORM ${PLATFORM} CACHE STRING "PX4 board OS" FORCE) + list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake) + + # platform-specific include path + include_directories(${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include) + endif() + + if(ARCHITECTURE) + set(CMAKE_SYSTEM_PROCESSOR ${ARCHITECTURE} CACHE INTERNAL "system processor" FORCE) + endif() + + if(TOOLCHAIN) + set(CMAKE_TOOLCHAIN_FILE Toolchain-${TOOLCHAIN} CACHE INTERNAL "toolchain file" FORCE) + endif() + + set(romfs_extra_files) + set(config_romfs_extra_dependencies) + # additional embedded metadata + if (NOT CONSTRAINED_FLASH AND NOT EXTERNAL_METADATA AND NOT ${PX4_BOARD_LABEL} STREQUAL "test") + list(APPEND romfs_extra_files + ${PX4_BINARY_DIR}/parameters.json.xz + ${PX4_BINARY_DIR}/events/all_events.json.xz + ${PX4_BINARY_DIR}/actuators.json.xz) + list(APPEND romfs_extra_dependencies + parameters_xml + events_json + actuators_json) + endif() + list(APPEND romfs_extra_files ${PX4_BINARY_DIR}/component_general.json.xz) + list(APPEND romfs_extra_dependencies component_general_json) + set(config_romfs_extra_files ${romfs_extra_files} CACHE INTERNAL "extra ROMFS files" FORCE) + set(config_romfs_extra_dependencies ${romfs_extra_dependencies} CACHE INTERNAL "extra ROMFS deps" FORCE) + + if(SERIAL_PORTS) + set(board_serial_ports ${SERIAL_PORTS} PARENT_SCOPE) + endif() + + # Serial ports + set(board_serial_ports) + if(SERIAL_URT6) + list(APPEND board_serial_ports URT6:${SERIAL_URT6}) + endif() + if(SERIAL_GPS1) + list(APPEND board_serial_ports GPS1:${SERIAL_GPS1}) + endif() + if(SERIAL_GPS2) + list(APPEND board_serial_ports GPS2:${SERIAL_GPS2}) + endif() + if(SERIAL_GPS3) + list(APPEND board_serial_ports GPS3:${SERIAL_GPS3}) + endif() + if(SERIAL_GPS4) + list(APPEND board_serial_ports GPS4:${SERIAL_GPS4}) + endif() + if(SERIAL_GPS5) + list(APPEND board_serial_ports GPS5:${SERIAL_GPS5}) + endif() + if(SERIAL_TEL1) + list(APPEND board_serial_ports TEL1:${SERIAL_TEL1}) + endif() + if(SERIAL_TEL2) + list(APPEND board_serial_ports TEL2:${SERIAL_TEL2}) + endif() + if(SERIAL_TEL3) + list(APPEND board_serial_ports TEL3:${SERIAL_TEL3}) + endif() + if(SERIAL_TEL4) + list(APPEND board_serial_ports TEL4:${SERIAL_TEL4}) + endif() + if(SERIAL_TEL5) + list(APPEND board_serial_ports TEL5:${SERIAL_TEL5}) + endif() + if(SERIAL_RC) + list(APPEND board_serial_ports RC:${SERIAL_RC}) + endif() + if(SERIAL_WIFI) + list(APPEND board_serial_ports WIFI:${SERIAL_WIFI}) + endif() + if(SERIAL_PPB) + list(APPEND board_serial_ports PPB:${SERIAL_PPB}) + endif() + + + # ROMFS + if(ROMFSROOT) + set(config_romfs_root ${ROMFSROOT} CACHE INTERNAL "ROMFS root" FORCE) + + if(UAVCAN_PERIPHERALS) + set(config_uavcan_peripheral_firmware ${UAVCAN_PERIPHERALS} CACHE INTERNAL "UAVCAN peripheral firmware" FORCE) + endif() + endif() + + if(UAVCAN_INTERFACES) + set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE) + endif() + + if(UAVCAN_TIMER_OVERRIDE) + set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE) + endif() + + # OPTIONS + + if(CONSTRAINED_FLASH) + set(px4_constrained_flash_build "1" CACHE INTERNAL "constrained flash build" FORCE) + add_definitions(-DCONSTRAINED_FLASH) + endif() + + if (NO_HELP) + add_definitions(-DCONSTRAINED_FLASH_NO_HELP="https://docs.px4.io/master/en/modules/modules_main.html") + endif() + + if(CONSTRAINED_MEMORY) + set(px4_constrained_memory_build "1" CACHE INTERNAL "constrained memory build" FORCE) + add_definitions(-DCONSTRAINED_MEMORY) + endif() + + if(TESTING) + set(PX4_TESTING "1" CACHE INTERNAL "testing enabled" FORCE) + endif() + + if(ETHERNET) + set(PX4_ETHERNET "1" CACHE INTERNAL "ethernet enabled" FORCE) + endif() + + if(CRYPTO) + set(PX4_CRYPTO "1" CACHE INTERNAL "PX4 crypto implementation" FORCE) + add_definitions(-DPX4_CRYPTO) + endif() + + if(LINKER_PREFIX) + set(PX4_BOARD_LINKER_PREFIX ${LINKER_PREFIX} CACHE STRING "PX4 board linker prefix" FORCE) + else() + set(PX4_BOARD_LINKER_PREFIX "" CACHE STRING "PX4 board linker prefix" FORCE) + endif() + + if(COMPILE_DEFINITIONS) + add_definitions( ${COMPILE_DEFINITIONS}) + endif() + + if(LINUX) + add_definitions( "-D__PX4_LINUX" ) + endif() + + if(LOCKSTEP) + set(ENABLE_LOCKSTEP_SCHEDULER yes) + endif() + + if(NOLOCKSTEP) + set(ENABLE_LOCKSTEP_SCHEDULER no) + endif() + + if(FULL_OPTIMIZATION) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Build type" FORCE) + endif() + + include(px4_impl_os) + px4_os_prebuild_targets(OUT prebuild_targets BOARD ${PX4_BOARD}) + + # add board config directory src to build modules + file(RELATIVE_PATH board_support_src_rel ${PX4_SOURCE_DIR}/src ${PX4_BOARD_DIR}) + list(APPEND config_module_list ${board_support_src_rel}/src) + + set(config_module_list ${config_module_list}) + set(config_kernel_list ${config_kernel_list}) + +endif() + + +if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") + add_custom_target(boardconfig + ${CMAKE_COMMAND} -E env + ${COMMON_KCONFIG_ENV_SETTINGS} + ${MENUCONFIG_PATH} Kconfig + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH} + COMMAND ${CMAKE_COMMAND} -E copy defconfig ${BOARD_DEFCONFIG} + COMMAND ${CMAKE_COMMAND} -E remove defconfig + COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + USES_TERMINAL + COMMAND_EXPAND_LISTS + ) + + add_custom_target(boardguiconfig + ${CMAKE_COMMAND} -E env + ${COMMON_KCONFIG_ENV_SETTINGS} + ${GUICONFIG_PATH} Kconfig + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH} + COMMAND ${CMAKE_COMMAND} -E copy defconfig ${BOARD_DEFCONFIG} + COMMAND ${CMAKE_COMMAND} -E remove defconfig + COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + USES_TERMINAL + COMMAND_EXPAND_LISTS + ) +else() + add_custom_target(boardconfig + ${CMAKE_COMMAND} -E env + ${COMMON_KCONFIG_ENV_SETTINGS} + ${MENUCONFIG_PATH} Kconfig + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH} + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG} + COMMAND ${CMAKE_COMMAND} -E remove defconfig + COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + USES_TERMINAL + COMMAND_EXPAND_LISTS + ) + + add_custom_target(boardguiconfig + ${CMAKE_COMMAND} -E env + ${COMMON_KCONFIG_ENV_SETTINGS} + ${GUICONFIG_PATH} Kconfig + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH} + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/diffconfig.py -m ${PX4_BOARD_DIR}/default.px4board defconfig > ${BOARD_DEFCONFIG} + COMMAND ${CMAKE_COMMAND} -E remove defconfig + COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + USES_TERMINAL + COMMAND_EXPAND_LISTS + ) +endif() diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index 48e8c22502a4..59ae6bd82990 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -45,14 +45,31 @@ add_custom_target(metadata_airframes USES_TERMINAL ) + +set(generated_params_dir ${PX4_BINARY_DIR}/generated_params_metadata) file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml ${PX4_SOURCE_DIR}/src/drivers/*.yaml ${PX4_SOURCE_DIR}/src/lib/*.yaml) + +# avoid param duplicates +list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/") +list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/") + add_custom_target(metadata_parameters COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs - COMMAND ${PYTHON_EXECUTABLE} - ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py --all-ports --params-file ${PX4_SOURCE_DIR}/src/generated_serial_params.c --config-files ${yaml_config_files} + COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir} + + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py + --all-ports --ethernet --params-file ${generated_params_dir}/serial_params.c --config-files ${yaml_config_files} + + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/module_config/generate_params.py + --params-file ${generated_params_dir}/module_params.c + --timer-config ${PX4_SOURCE_DIR}/boards/px4/fmu-v5/src/timer_config.cpp # select a typical board + --board-with-io + --ethernet + --config-files ${yaml_config_files} #--verbose + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --markdown ${PX4_BINARY_DIR}/docs/parameters.md @@ -79,9 +96,29 @@ add_custom_target(metadata_module_documentation USES_TERMINAL ) +set(events_src_path "${PX4_SOURCE_DIR}/src/lib/events") +add_custom_target(metadata_extract_events + COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/events + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py + --src-path ${PX4_SOURCE_DIR}/src + --json ${PX4_BINARY_DIR}/events/px4_full.json #--verbose + COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/combine.py + ${PX4_BINARY_DIR}/events/px4_full.json + ${events_src_path}/libevents/events/common.json + ${events_src_path}/enums.json + --output ${PX4_BINARY_DIR}/events/all_events_full.json + COMMAND ${PYTHON_EXECUTABLE} ${events_src_path}/libevents/scripts/validate.py + ${PX4_BINARY_DIR}/events/all_events_full.json + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py + ${PX4_BINARY_DIR}/events/all_events_full.json + COMMENT "Extracting events from full source" + USES_TERMINAL +) + add_custom_target(all_metadata DEPENDS metadata_airframes metadata_parameters metadata_module_documentation + metadata_extract_events ) diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake deleted file mode 100644 index ef65b7096447..000000000000 --- a/cmake/px4_add_board.cmake +++ /dev/null @@ -1,317 +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. -# -############################################################################ - -#============================================================================= -# -# px4_add_board -# -# This function creates a PX4 board. -# -# Usage: -# px4_add_board( -# PLATFORM -# VENDOR -# MODEL -# [ LABEL ] -# [ TOOLCHAIN ] -# [ ARCHITECTURE ] -# [ ROMFSROOT ] -# [ BUILD_BOOTLOADER ] -# [ IO ] -# [ UAVCAN_INTERFACES ] -# [ UAVCAN_PERIPHERALS ] -# [ DRIVERS ] -# [ MODULES ] -# [ SYSTEMCMDS ] -# [ EXAMPLES ] -# [ SERIAL_PORTS ] -# [ CONSTRAINED_FLASH ] -# [ CONSTRAINED_MEMORY ] -# [ TESTING ] -# [ LINKER_PREFIX ] -# [ EMBEDDED_METADATA ] -# [ ETHERNET ] -# ) -# -# Input: -# PLATFORM : PX4 platform name (posix, nuttx, qurt) -# VENDOR : name of board vendor/manufacturer/brand/etc -# MODEL : name of board model -# LABEL : optional label, set to default if not specified -# TOOLCHAIN : cmake toolchain -# ARCHITECTURE : name of the CPU CMake is building for (used by the toolchain) -# ROMFSROOT : relative path to the ROMFS root directory -# BUILD_BOOTLOADER : flag to enable building and including the bootloader config -# IO : name of IO board to be built and included in the ROMFS (requires a valid ROMFSROOT) -# UAVCAN_INTERFACES : number of interfaces for UAVCAN -# UAVCAN_PERIPHERALS : list of UAVCAN peripheral firmware to build and embed -# DRIVERS : list of drivers to build for this board (relative to src/drivers) -# MODULES : list of modules to build for this board (relative to src/modules) -# SYSTEMCMDS : list of system commands to build for this board (relative to src/systemcmds) -# EXAMPLES : list of example modules to build for this board (relative to src/examples) -# SERIAL_PORTS : mapping of user configurable serial ports and param facing name -# EMBEDDED_METADATA : list of metadata to embed to ROMFS -# CONSTRAINED_FLASH : flag to enable constrained flash options (eg limit init script status text) -# CONSTRAINED_MEMORY : flag to enable constrained memory options (eg limit maximum number of uORB publications) -# TESTING : flag to enable automatic inclusion of PX4 testing modules -# LINKER_PREFIX : optional to prefix on the Linker script. -# ETHERNET : flag to indicate that ethernet is enabled -# -# -# Example: -# px4_add_board( -# PLATFORM nuttx -# VENDOR px4 -# MODEL fmu-v5 -# TOOLCHAIN arm-none-eabi -# ARCHITECTURE cortex-m7 -# ROMFSROOT px4fmu_common -# IO px4_io-v2_default -# SERIAL_PORTS -# GPS1:/dev/ttyS0 -# TEL1:/dev/ttyS1 -# TEL2:/dev/ttyS2 -# TEL4:/dev/ttyS3 -# DRIVERS -# barometer/ms5611 -# gps -# imu/bosch/bmi055 -# imu/invensense/mpu6000 -# magnetometer/isentek/ist8310 -# pwm_out -# px4io -# rgbled -# MODULES -# commander -# ekf2 -# land_detector -# mavlink -# mc_att_control -# mc_pos_control -# navigator -# sensors -# MODULES -# mixer -# mtd -# param -# perf -# pwm -# reboot -# shutdown -# top -# topic_listener -# tune_control -# ) -# -function(px4_add_board) - - px4_parse_function_args( - NAME px4_add_board - ONE_VALUE - PLATFORM - VENDOR - MODEL - LABEL - TOOLCHAIN - ARCHITECTURE - ROMFSROOT - IO - UAVCAN_INTERFACES - UAVCAN_TIMER_OVERRIDE - LINKER_PREFIX - MULTI_VALUE - DRIVERS - MODULES - SYSTEMCMDS - EXAMPLES - SERIAL_PORTS - UAVCAN_PERIPHERALS - EMBEDDED_METADATA - OPTIONS - BUILD_BOOTLOADER - CONSTRAINED_FLASH - CONSTRAINED_MEMORY - TESTING - ETHERNET - REQUIRED - PLATFORM - VENDOR - MODEL - ARGN ${ARGN}) - - set(PX4_BOARD_DIR ${CMAKE_CURRENT_LIST_DIR} CACHE STRING "PX4 board directory" FORCE) - include_directories(${PX4_BOARD_DIR}/src) - - set(PX4_BOARD ${VENDOR}_${MODEL} CACHE STRING "PX4 board" FORCE) - - # board name is uppercase with no underscores when used as a define - string(TOUPPER ${PX4_BOARD} PX4_BOARD_NAME) - string(REPLACE "-" "_" PX4_BOARD_NAME ${PX4_BOARD_NAME}) - set(PX4_BOARD_NAME ${PX4_BOARD_NAME} CACHE STRING "PX4 board define" FORCE) - - set(PX4_BOARD_VENDOR ${VENDOR} CACHE STRING "PX4 board vendor" FORCE) - set(PX4_BOARD_MODEL ${MODEL} CACHE STRING "PX4 board model" FORCE) - - if(LABEL) - set(PX4_BOARD_LABEL ${LABEL} CACHE STRING "PX4 board label" FORCE) - else() - set(PX4_BOARD_LABEL "default" CACHE STRING "PX4 board label" FORCE) - endif() - - set(PX4_CONFIG "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}" CACHE STRING "PX4 config" FORCE) - - # set OS, and append specific platform module path - set(PX4_PLATFORM ${PLATFORM} CACHE STRING "PX4 board OS" FORCE) - list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake) - - # platform-specific include path - include_directories(${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include) - - if(ARCHITECTURE) - set(CMAKE_SYSTEM_PROCESSOR ${ARCHITECTURE} CACHE INTERNAL "system processor" FORCE) - endif() - - if(TOOLCHAIN) - set(CMAKE_TOOLCHAIN_FILE Toolchain-${TOOLCHAIN} CACHE INTERNAL "toolchain file" FORCE) - endif() - - set(romfs_extra_files) - set(config_romfs_extra_dependencies) - foreach(metadata ${EMBEDDED_METADATA}) - if(${metadata} STREQUAL "parameters") - list(APPEND romfs_extra_files ${PX4_BINARY_DIR}/parameters.json.xz) - list(APPEND romfs_extra_dependencies parameters_xml) - else() - message(FATAL_ERROR "invalid value for EMBEDDED_METADATA: ${metadata}") - endif() - endforeach() - list(APPEND romfs_extra_files ${PX4_BINARY_DIR}/component_general.json.xz) - list(APPEND romfs_extra_dependencies component_general_json) - set(config_romfs_extra_files ${romfs_extra_files} CACHE INTERNAL "extra ROMFS files" FORCE) - set(config_romfs_extra_dependencies ${romfs_extra_dependencies} CACHE INTERNAL "extra ROMFS deps" FORCE) - - if(SERIAL_PORTS) - set(board_serial_ports ${SERIAL_PORTS} PARENT_SCOPE) - endif() - - # ROMFS - if(ROMFSROOT) - set(config_romfs_root ${ROMFSROOT} CACHE INTERNAL "ROMFS root" FORCE) - - if(BUILD_BOOTLOADER) - set(config_build_bootloader "1" CACHE INTERNAL "build bootloader" FORCE) - endif() - - # IO board (placed in ROMFS) - if(IO) - set(config_io_board ${IO} CACHE INTERNAL "IO" FORCE) - endif() - - if(UAVCAN_PERIPHERALS) - set(config_uavcan_peripheral_firmware ${UAVCAN_PERIPHERALS} CACHE INTERNAL "UAVCAN peripheral firmware" FORCE) - endif() - endif() - - if(UAVCAN_INTERFACES) - set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE) - endif() - - if(UAVCAN_TIMER_OVERRIDE) - set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE) - endif() - - # OPTIONS - - if(CONSTRAINED_FLASH) - set(px4_constrained_flash_build "1" CACHE INTERNAL "constrained flash build" FORCE) - add_definitions(-DCONSTRAINED_FLASH) - endif() - - if(CONSTRAINED_MEMORY) - set(px4_constrained_memory_build "1" CACHE INTERNAL "constrained memory build" FORCE) - add_definitions(-DCONSTRAINED_MEMORY) - endif() - - if(TESTING) - set(PX4_TESTING "1" CACHE INTERNAL "testing enabled" FORCE) - endif() - - if(ETHERNET) - set(PX4_ETHERNET "1" CACHE INTERNAL "ethernet enabled" FORCE) - endif() - - if(LINKER_PREFIX) - set(PX4_BOARD_LINKER_PREFIX ${LINKER_PREFIX} CACHE STRING "PX4 board linker prefix" FORCE) - else() - set(PX4_BOARD_LINKER_PREFIX "" CACHE STRING "PX4 board linker prefix" FORCE) - endif() - - include(px4_impl_os) - px4_os_prebuild_targets(OUT prebuild_targets BOARD ${PX4_BOARD}) - - - ########################################################################### - # Modules (includes drivers, examples, modules, systemcmds) - set(config_module_list) - - if(DRIVERS) - foreach(driver ${DRIVERS}) - list(APPEND config_module_list drivers/${driver}) - endforeach() - endif() - - if(MODULES) - foreach(module ${MODULES}) - list(APPEND config_module_list modules/${module}) - endforeach() - endif() - - if(SYSTEMCMDS) - foreach(systemcmd ${SYSTEMCMDS}) - list(APPEND config_module_list systemcmds/${systemcmd}) - endforeach() - endif() - - if(EXAMPLES) - foreach(example ${EXAMPLES}) - list(APPEND config_module_list examples/${example}) - endforeach() - endif() - - # add board config directory src to build modules - file(RELATIVE_PATH board_support_src_rel ${PX4_SOURCE_DIR}/src ${PX4_BOARD_DIR}) - list(APPEND config_module_list ${board_support_src_rel}/src) - - set(config_module_list ${config_module_list} PARENT_SCOPE) - -endfunction() diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index 8f45c71109ac..71fbbf26578b 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -95,21 +95,17 @@ function(px4_add_common_flags) # compiler specific flags if (("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") OR ("${CMAKE_CXX_COMPILER_ID}" MATCHES "AppleClang")) - # force color for clang (needed for clang + ccache) - add_compile_options(-fcolor-diagnostics) - # force absolute paths - add_compile_options(-fdiagnostics-absolute-paths) - - # QuRT 6.4.X compiler identifies as Clang but does not support this option - if (NOT "${PX4_PLATFORM}" STREQUAL "qurt") - add_compile_options( - -Qunused-arguments - - -Wno-unknown-warning-option - -Wno-unused-const-variable - -Wno-varargs - ) - endif() + add_compile_options( + -fcolor-diagnostics # force color for clang (needed for clang + ccache) + -fdiagnostics-absolute-paths # force absolute paths + + -Qunused-arguments + + -Wno-c99-designator + -Wno-unknown-warning-option + -Wno-unused-const-variable + -Wno-varargs + ) elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") @@ -161,7 +157,6 @@ function(px4_add_common_flags) set(cxx_flags) list(APPEND cxx_flags -fno-exceptions - -fno-rtti -fno-threadsafe-statics -Wreorder @@ -169,6 +164,13 @@ function(px4_add_common_flags) # disabled warnings -Wno-overloaded-virtual # TODO: fix and remove ) + + if(NOT CMAKE_BUILD_TYPE STREQUAL FuzzTesting) + list(APPEND cxx_flags + -fno-rtti + ) + endif() + foreach(flag ${cxx_flags}) add_compile_options($<$:${flag}>) endforeach() diff --git a/cmake/px4_add_library.cmake b/cmake/px4_add_library.cmake index d77af0928100..7dc7b6cfd013 100644 --- a/cmake/px4_add_library.cmake +++ b/cmake/px4_add_library.cmake @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +include(px4_list_make_absolute) #============================================================================= # @@ -43,13 +44,10 @@ function(px4_add_library target) target_compile_definitions(${target} PRIVATE MODULE_NAME="${target}") # all PX4 libraries have access to parameters and uORB - add_dependencies(${target} uorb_headers) - target_link_libraries(${target} PRIVATE prebuild_targets parameters_interface px4_platform uorb_msgs) - - # TODO: move to platform layer - if ("${PX4_PLATFORM}" MATCHES "nuttx") - target_link_libraries(${target} PRIVATE m nuttx_c) - endif() + add_dependencies(${target} uorb_headers parameters) + target_link_libraries(${target} PRIVATE prebuild_targets) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${ARGN}) + set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS}) endfunction() diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index ba3a74d9ec8f..d07c98df0012 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +include(px4_list_make_absolute) #============================================================================= # @@ -106,7 +107,7 @@ function(px4_add_module) # unity build add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp COMMAND cat ${SRCS} > ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp - DEPENDS ${MODULE}_original ${DEPENDS} ${SRCS} + DEPENDS ${SRCS} COMMENT "${MODULE} merging source" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) @@ -114,6 +115,7 @@ function(px4_add_module) add_library(${MODULE} STATIC EXCLUDE_FROM_ALL ${CMAKE_CURRENT_BINARY_DIR}/${MODULE}_unity.cpp) target_include_directories(${MODULE} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}) + add_dependencies(${MODULE} ${MODULE}_original) # build standalone module first to get clean compile errors if(COMPILE_FLAGS) target_compile_options(${MODULE}_original PRIVATE ${COMPILE_FLAGS}) @@ -124,7 +126,7 @@ function(px4_add_module) # as well as interface include and libraries foreach(dep ${DEPENDS}) get_target_property(dep_type ${dep} TYPE) - if (${dep_type} STREQUAL "STATIC_LIBRARY") + if((${dep_type} STREQUAL "STATIC_LIBRARY") OR (${dep_type} STREQUAL "INTERFACE_LIBRARY")) target_link_libraries(${MODULE}_original PRIVATE ${dep}) else() add_dependencies(${MODULE}_original ${dep}) @@ -152,12 +154,35 @@ function(px4_add_module) # all modules can potentially use parameters and uORB add_dependencies(${MODULE} uorb_headers) + # Check if the modules source dir exists in config_kernel_list + # in this case, treat is as a kernel side component for + # protected build + get_target_property(MODULE_SOURCE_DIR ${MODULE} SOURCE_DIR) + file(RELATIVE_PATH module ${PROJECT_SOURCE_DIR}/src ${MODULE_SOURCE_DIR}) + + list (FIND config_kernel_list ${module} _index) + if (${_index} GREATER -1) + set (KERNEL TRUE) + endif() + if(NOT DYNAMIC) - target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_layer px4_platform systemlib) - set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) + target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf) + if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL) + target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel) + set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE}) + else() + target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB) + set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) + endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS}) + set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS}) endif() + set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS}) + set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS}) + # set defaults if not set set(MAIN_DEFAULT MAIN-NOTFOUND) set(STACK_MAIN_DEFAULT 2048) @@ -176,9 +201,7 @@ function(px4_add_module) endif() set_target_properties(${MODULE} PROPERTIES STACK_MAX ${STACK_MAX}) - if(${PX4_PLATFORM} STREQUAL "qurt") - set_property(TARGET ${MODULE} PROPERTY POSITION_INDEPENDENT_CODE TRUE) - elseif(${PX4_PLATFORM} STREQUAL "nuttx") + if(${PX4_PLATFORM} STREQUAL "nuttx") target_compile_options(${MODULE} PRIVATE -Wframe-larger-than=${STACK_MAX}) endif() @@ -194,6 +217,10 @@ function(px4_add_module) target_compile_options(${MODULE} PRIVATE ${COMPILE_FLAGS}) endif() + if (KERNEL) + target_compile_options(${MODULE} PRIVATE -D__KERNEL__) + endif() + if(INCLUDES) target_include_directories(${MODULE} PRIVATE ${INCLUDES}) endif() @@ -203,7 +230,7 @@ function(px4_add_module) # as well as interface include and libraries foreach(dep ${DEPENDS}) get_target_property(dep_type ${dep} TYPE) - if (${dep_type} STREQUAL "STATIC_LIBRARY") + if((${dep_type} STREQUAL "STATIC_LIBRARY") OR (${dep_type} STREQUAL "INTERFACE_LIBRARY")) target_link_libraries(${MODULE} PRIVATE ${dep}) else() add_dependencies(${MODULE} ${dep}) diff --git a/cmake/px4_config.cmake b/cmake/px4_config.cmake index 58e3db8e6af8..41ea7b8c076a 100644 --- a/cmake/px4_config.cmake +++ b/cmake/px4_config.cmake @@ -37,7 +37,7 @@ if(NOT PX4_CONFIG_FILE) file(GLOB_RECURSE board_configs RELATIVE "${PX4_SOURCE_DIR}/boards" - "boards/*.cmake" + "boards/*.px4board" ) set(PX4_CONFIGS ${board_configs} CACHE STRING "PX4 board configs" FORCE) @@ -45,7 +45,7 @@ if(NOT PX4_CONFIG_FILE) foreach(filename ${board_configs}) # parse input CONFIG into components to match with existing in tree configs # the platform prefix (eg nuttx_) is historical, and removed if present - string(REPLACE ".cmake" "" filename_stripped ${filename}) + string(REPLACE ".px4board" "" filename_stripped ${filename}) string(REPLACE "/" ";" config ${filename_stripped}) list(LENGTH config config_len) @@ -62,6 +62,10 @@ if(NOT PX4_CONFIG_FILE) ((${label} STREQUAL "default") AND (${CONFIG} STREQUAL "${vendor}_${model}")) # default label can be omitted ) set(PX4_CONFIG_FILE "${PX4_SOURCE_DIR}/boards/${filename}" CACHE FILEPATH "path to PX4 CONFIG file" FORCE) + set(PX4_BOARD_DIR "${PX4_SOURCE_DIR}/boards/${vendor}/${model}" CACHE STRING "PX4 board directory" FORCE) + set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE) + set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE) + set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE) break() endif() @@ -71,6 +75,10 @@ if(NOT PX4_CONFIG_FILE) ((${label} STREQUAL "default") AND (${CONFIG} STREQUAL "${board}")) # default label can be omitted ) set(PX4_CONFIG_FILE "${PX4_SOURCE_DIR}/boards/${filename}" CACHE FILEPATH "path to PX4 CONFIG file" FORCE) + set(PX4_BOARD_DIR "${PX4_SOURCE_DIR}/boards/${vendor}/${model}" CACHE STRING "PX4 board directory" FORCE) + set(MODEL "${model}" CACHE STRING "PX4 board model" FORCE) + set(VENDOR "${vendor}" CACHE STRING "PX4 board vendor" FORCE) + set(LABEL "${label}" CACHE STRING "PX4 board vendor" FORCE) break() endif() endif() @@ -82,3 +90,27 @@ if(NOT PX4_CONFIG_FILE) endif() message(STATUS "PX4 config file: ${PX4_CONFIG_FILE}") + +include_directories(${PX4_BOARD_DIR}/src) + +set(PX4_BOARD ${VENDOR}_${MODEL} CACHE STRING "PX4 board" FORCE) + +# board name is uppercase with no underscores when used as a define +string(TOUPPER ${PX4_BOARD} PX4_BOARD_NAME) +string(REPLACE "-" "_" PX4_BOARD_NAME ${PX4_BOARD_NAME}) +set(PX4_BOARD_NAME ${PX4_BOARD_NAME} CACHE STRING "PX4 board define" FORCE) + +set(PX4_BOARD_VENDOR ${VENDOR} CACHE STRING "PX4 board vendor" FORCE) +set(PX4_BOARD_MODEL ${MODEL} CACHE STRING "PX4 board model" FORCE) + +set(PX4_BOARD_LABEL ${LABEL} CACHE STRING "PX4 board label" FORCE) + +set(PX4_CONFIG "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}" CACHE STRING "PX4 config" FORCE) + +if(EXISTS "${PX4_BOARD_DIR}/uavcan_board_identity") +include ("${PX4_BOARD_DIR}/uavcan_board_identity") +endif() + +if(EXISTS "${PX4_BOARD_DIR}/sitl.cmake") +include ("${PX4_BOARD_DIR}/sitl.cmake") +endif() diff --git a/cmake/px4_list_make_absolute.cmake b/cmake/px4_list_make_absolute.cmake new file mode 100644 index 000000000000..3964b7bd4e18 --- /dev/null +++ b/cmake/px4_list_make_absolute.cmake @@ -0,0 +1,57 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# cmake include guard +if(px4_list_make_absolute_included) + return() +endif(px4_list_make_absolute_included) +set(px4_list_make_absolute_included true) + +#============================================================================= +# +# px4_list_make_absolute +# +# prepend a prefix to each element in a list, if the element is not an abolute +# path +# +function(px4_list_make_absolute var prefix) + set(list_var "") + foreach(f ${ARGN}) + if(IS_ABSOLUTE ${f}) + list(APPEND list_var "${f}") + else() + list(APPEND list_var "${prefix}/${f}") + endif() + endforeach(f) + set(${var} "${list_var}" PARENT_SCOPE) +endfunction() diff --git a/cmake/sanitizers.cmake b/cmake/sanitizers.cmake index 435944bfc110..612af9f8bd28 100644 --- a/cmake/sanitizers.cmake +++ b/cmake/sanitizers.cmake @@ -130,6 +130,24 @@ elseif (CMAKE_BUILD_TYPE STREQUAL UndefinedBehaviorSanitizer) function(sanitizer_fail_test_on_error test_name) set_tests_properties(${test_name} PROPERTIES FAIL_REGULAR_EXPRESSION "runtime error:") endfunction(sanitizer_fail_test_on_error) + +elseif (CMAKE_BUILD_TYPE STREQUAL FuzzTesting) + message(STATUS "FuzzTesting enabled") + + add_compile_options( + -g3 + -fsanitize=fuzzer + -DFUZZTESTING + ) + + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fsanitize=fuzzer $ENV{LIB_FUZZING_ENGINE}" CACHE INTERNAL "" FORCE) + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fsanitize=fuzzer $ENV{LIB_FUZZING_ENGINE}" CACHE INTERNAL "" FORCE) + set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -fsanitize=fuzzer $ENV{LIB_FUZZING_ENGINE}" CACHE INTERNAL "" FORCE) + + + function(sanitizer_fail_test_on_error test_name) + # Not sure what to do here + endfunction(sanitizer_fail_test_on_error) else() function(sanitizer_fail_test_on_error test_name) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py index c99a649d9038..836b0da3b68b 100755 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py @@ -45,6 +45,7 @@ import math import numpy as np from geometry_msgs.msg import PoseStamped, Quaternion +from mavros_msgs.msg import ParamValue from mavros_test_common import MavrosTestCommon from pymavlink import mavutil from six.moves import xrange @@ -163,6 +164,9 @@ def test_posctl(self): 10, -1) self.log_topic_vars() + # exempting failsafe from lost RC to allow offboard + rcl_except = ParamValue(1<<2, 0.0) + self.set_param("COM_RCL_EXCEPT", rcl_except, 5) self.set_mode("OFFBOARD", 5) self.set_arm(True, 5) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py index ef334e1f3819..642d66933572 100644 --- a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py +++ b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py @@ -5,9 +5,9 @@ import rospy import math from geometry_msgs.msg import PoseStamped -from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, State, \ +from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, ParamValue, State, \ WaypointList -from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \ +from mavros_msgs.srv import CommandBool, ParamGet, ParamSet, SetMode, SetModeRequest, WaypointClear, \ WaypointPush from pymavlink import mavutil from sensor_msgs.msg import NavSatFix, Imu @@ -42,6 +42,7 @@ def setUp(self): rospy.loginfo("waiting for ROS services") try: rospy.wait_for_service('mavros/param/get', service_timeout) + rospy.wait_for_service('mavros/param/set', service_timeout) rospy.wait_for_service('mavros/cmd/arming', service_timeout) rospy.wait_for_service('mavros/mission/push', service_timeout) rospy.wait_for_service('mavros/mission/clear', service_timeout) @@ -50,6 +51,7 @@ def setUp(self): except rospy.ROSException: self.fail("failed to connect to services") self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) + self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet) self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) @@ -234,6 +236,36 @@ def set_mode(self, mode, timeout): "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}". format(mode, old_mode, timeout))) + def set_param(self, param_id, param_value, timeout): + """param: PX4 param string, ParamValue, timeout(int): seconds""" + if param_value.integer != 0: + value = param_value.integer + else: + value = param_value.real + rospy.loginfo("setting PX4 parameter: {0} with value {1}". + format(param_id, value)) + loop_freq = 1 # Hz + rate = rospy.Rate(loop_freq) + param_set = False + for i in xrange(timeout * loop_freq): + try: + res = self.set_param_srv(param_id, param_value) + if res.success: + rospy.loginfo("param {0} set to {1} | seconds: {2} of {3}". + format(param_id, value, i / loop_freq, timeout)) + break + except rospy.ServiceException as e: + rospy.logerr(e) + + try: + rate.sleep() + except rospy.ROSException as e: + self.fail(e) + + self.assertTrue(res.success, ( + "failed to set param | param_id: {0}, param_value: {1} | timeout(seconds): {2}". + format(param_id, value, timeout))) + def wait_for_topics(self, timeout): """wait for simulation to be ready, make sure we're getting topic info from all topics by checking dictionary of flag values set in callbacks, diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 deleted file mode 160000 index 9e07c7d0b6eb..000000000000 --- a/mavlink/include/mavlink/v2.0 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9e07c7d0b6eb91f0fd84f911dc91e68e865ba7ed diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b66c6e41b5b5..fcd004f85fa2 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -34,16 +34,27 @@ # Support IN_LIST if() operator cmake_policy(SET CMP0057 NEW) +include(px4_list_make_absolute) + set(msg_files + action_request.msg actuator_armed.msg actuator_controls.msg + actuator_controls_status.msg + actuator_motors.msg actuator_outputs.msg + actuator_servos.msg + actuator_servos_trim.msg + actuator_test.msg adc_report.msg airspeed.msg airspeed_validated.msg airspeed_wind.msg + autotune_attitude_control_status.msg battery_status.msg + button_event.msg camera_capture.msg + camera_status.msg camera_trigger.msg cellular_status.msg collision_constraints.msg @@ -54,18 +65,25 @@ set(msg_files differential_pressure.msg distance_sensor.msg ekf2_timestamps.msg - ekf_gps_drift.msg esc_report.msg esc_status.msg + estimator_aid_source_1d.msg + estimator_aid_source_2d.msg + estimator_aid_source_3d.msg + estimator_baro_bias.msg estimator_event_flags.msg + estimator_gps_status.msg estimator_innovations.msg - estimator_optical_flow_vel.msg estimator_selector_status.msg estimator_sensor_bias.msg estimator_states.msg estimator_status.msg estimator_status_flags.msg + event.msg + failure_detector_status.msg follow_target.msg + follow_target_estimator.msg + follow_target_status.msg generator_status.msg geofence_result.msg gimbal_device_attitude_status.msg @@ -81,6 +99,7 @@ set(msg_files home_position.msg hover_thrust_estimate.msg input_rc.msg + internal_combustion_engine_status.msg iridiumsbd_status.msg irlock_report.msg landing_gear.msg @@ -90,18 +109,19 @@ set(msg_files log_message.msg logger_status.msg mag_worker_data.msg + magnetometer_bias_estimate.msg manual_control_setpoint.msg manual_control_switches.msg mavlink_log.msg + mavlink_tunnel.msg mission.msg mission_result.msg mount_orientation.msg - multirotor_motor_limits.msg navigator_mission_item.msg + npfg_status.msg obstacle_distance.msg offboard_control_mode.msg onboard_computer_status.msg - optical_flow.msg orbit_status.msg parameter_update.msg ping.msg @@ -111,30 +131,32 @@ set(msg_files position_setpoint_triplet.msg power_button_state.msg power_monitor.msg + pps_capture.msg pwm_input.msg px4io_status.msg - qshell_req.msg - qshell_retval.msg radio_status.msg rate_ctrl_status.msg rc_channels.msg rc_parameter_map.msg rpm.msg - rtl_flight_time.msg - safety.msg + rtl_time_estimate.msg satellite_info.msg sensor_accel.msg sensor_accel_fifo.msg sensor_baro.msg sensor_combined.msg sensor_correction.msg + sensor_gnss_relative.msg sensor_gps.msg sensor_gyro.msg sensor_gyro_fft.msg sensor_gyro_fifo.msg + sensor_hygrometer.msg sensor_mag.msg + sensor_optical_flow.msg sensor_preflight_mag.msg sensor_selection.msg + sensors_status.msg sensors_status_imu.msg system_power.msg takeoff_status.msg @@ -145,6 +167,7 @@ set(msg_files timesync.msg timesync_status.msg trajectory_bezier.msg + trajectory_setpoint.msg trajectory_waypoint.msg transponder_report.msg tune_control.msg @@ -152,8 +175,9 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg + uwb_distance.msg + uwb_grid.msg vehicle_acceleration.msg - vehicle_actuator_setpoint.msg vehicle_air_data.msg vehicle_angular_acceleration.msg vehicle_angular_acceleration_setpoint.msg @@ -173,6 +197,8 @@ set(msg_files vehicle_local_position_setpoint.msg vehicle_magnetometer.msg vehicle_odometry.msg + vehicle_optical_flow.msg + vehicle_optical_flow_vel.msg vehicle_rates_setpoint.msg vehicle_roi.msg vehicle_status.msg @@ -229,6 +255,8 @@ if(invalid_msgs) endif() endif() +px4_list_make_absolute(msg_files ${CMAKE_CURRENT_SOURCE_DIR} ${msg_files}) + if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "") # Check that the msg directory and the CMakeLists.txt file exists if(EXISTS ${EXTERNAL_MODULES_LOCATION}/msg/CMakeLists.txt) @@ -243,14 +271,17 @@ endif() # headers set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics) +set(ucdr_out_path ${PX4_BINARY_DIR}/uORB/ucdr) set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) set(uorb_headers ${msg_out_path}/uORBTopics.hpp) set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp) +set(uorb_ucdr_headers) foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) list(APPEND uorb_headers ${msg_out_path}/${msg}.h) list(APPEND uorb_sources ${msg_source_out_path}/${msg}.cpp) + list(APPEND uorb_ucdr_headers ${ucdr_out_path}/${msg}.h) endforeach() if (px4_constrained_flash_build) @@ -283,6 +314,27 @@ add_custom_command(OUTPUT ${uorb_headers} ) add_custom_target(uorb_headers DEPENDS ${uorb_headers}) +add_custom_command(OUTPUT ${uorb_ucdr_headers} + COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py + --headers + -f ${msg_files} + -i ${CMAKE_CURRENT_SOURCE_DIR} + -o ${ucdr_out_path} + -e templates/ucdr + -t ${CMAKE_CURRENT_BINARY_DIR}/tmp/ucdr_headers + -q + ${added_arguments} + DEPENDS + ${msg_files} + templates/ucdr/msg.h.em + tools/px_generate_uorb_topic_files.py + tools/px_generate_uorb_topic_helper.py + COMMENT "Generating uORB topic ucdr headers" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + VERBATIM + ) +add_custom_target(uorb_ucdr_headers DEPENDS ${uorb_ucdr_headers}) + # Generate uORB sources add_custom_command(OUTPUT ${uorb_sources} COMMAND ${PYTHON_EXECUTABLE} tools/px_generate_uorb_topic_files.py @@ -306,5 +358,4 @@ add_custom_command(OUTPUT ${uorb_sources} ) add_library(uorb_msgs ${uorb_sources}) -target_link_libraries(uorb_msgs PRIVATE m) add_dependencies(uorb_msgs prebuild_targets uorb_headers) diff --git a/msg/action_request.msg b/msg/action_request.msg new file mode 100644 index 000000000000..a9c2f7258fe5 --- /dev/null +++ b/msg/action_request.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 action # what action is requested +uint8 ACTION_DISARM = 0 +uint8 ACTION_ARM = 1 +uint8 ACTION_TOGGLE_ARMING = 2 +uint8 ACTION_UNKILL = 3 +uint8 ACTION_KILL = 4 +uint8 ACTION_SWITCH_MODE = 5 +uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6 +uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7 + +uint8 source # how the request was triggered +uint8 SOURCE_RC_STICK_GESTURE = 0 +uint8 SOURCE_RC_SWITCH = 1 +uint8 SOURCE_RC_BUTTON = 2 +uint8 SOURCE_RC_MODE_SLOT = 3 + +uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to commander_state.MAIN_STATE_ diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index c4b52ea48dfe..ec8203c9ab8c 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -1,6 +1,6 @@ uint64 timestamp # time since system start (microseconds) uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4 uint8 INDEX_ROLL = 0 uint8 INDEX_PITCH = 1 uint8 INDEX_YAW = 2 @@ -11,18 +11,16 @@ uint8 INDEX_AIRBRAKES = 6 uint8 INDEX_LANDING_GEAR = 7 uint8 INDEX_GIMBAL_SHUTTER = 3 uint8 INDEX_CAMERA_ZOOM = 4 +uint8 INDEX_COLLECTIVE_TILT = 8 uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 uint8 GROUP_INDEX_GIMBAL = 2 uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3 -uint8 GROUP_INDEX_ALLOCATED_PART1 = 4 -uint8 GROUP_INDEX_ALLOCATED_PART2 = 5 uint8 GROUP_INDEX_PAYLOAD = 6 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control +float32[9] control # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 -# TOPICS actuator_controls_4 actuator_controls_5 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/actuator_controls_status.msg b/msg/actuator_controls_status.msg new file mode 100644 index 000000000000..4e3bf8345011 --- /dev/null +++ b/msg/actuator_controls_status.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 + +float32[4] control_power + +# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1 diff --git a/msg/actuator_motors.msg b/msg/actuator_motors.msg new file mode 100644 index 000000000000..e74566f1f75e --- /dev/null +++ b/msg/actuator_motors.msg @@ -0,0 +1,12 @@ +# Motor control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint16 reversible_flags # bitset which motors are configured to be reversible + +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 + +uint8 NUM_CONTROLS = 12 +float32[12] control # range: [-1, 1], where 1 means maximum positive thrust, + # -1 maximum negative (if not supported by the output, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) diff --git a/msg/actuator_outputs.msg b/msg/actuator_outputs.msg index ff546948d804..c16b73b5d72a 100644 --- a/msg/actuator_outputs.msg +++ b/msg/actuator_outputs.msg @@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS = 16 uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking uint32 noutputs # valid outputs float32[16] output # output data, in natural output units + +# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) +# TOPICS actuator_outputs actuator_outputs_sim diff --git a/msg/actuator_servos.msg b/msg/actuator_servos.msg new file mode 100644 index 000000000000..2c7900e8116a --- /dev/null +++ b/msg/actuator_servos.msg @@ -0,0 +1,8 @@ +# Servo control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_CONTROLS = 8 +float32[8] control # range: [-1, 1], where 1 means maximum positive position, + # -1 maximum negative, + # and NaN maps to disarmed diff --git a/msg/actuator_servos_trim.msg b/msg/actuator_servos_trim.msg new file mode 100644 index 000000000000..30953e7ae53e --- /dev/null +++ b/msg/actuator_servos_trim.msg @@ -0,0 +1,5 @@ +# Servo trims, added as offset to servo outputs +uint64 timestamp # time since system start (microseconds) + +uint8 NUM_CONTROLS = 8 +float32[8] trim # range: [-1, 1] diff --git a/msg/actuator_test.msg b/msg/actuator_test.msg new file mode 100644 index 000000000000..f0f0d52c97e4 --- /dev/null +++ b/msg/actuator_test.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +# Topic to test individual actuator output functions +# Note: this is only used with SYS_CTRL_ALLOC=1 + +uint8 ACTION_RELEASE_CONTROL = 0 # exit test mode for the given function +uint8 ACTION_DO_CONTROL = 1 # enable actuator test mode + +uint8 FUNCTION_MOTOR1 = 101 +uint8 MAX_NUM_MOTORS = 12 +uint8 FUNCTION_SERVO1 = 201 +uint8 MAX_NUM_SERVOS = 8 + +uint8 action # one of ACTION_* +uint16 function # actuator output function +float32 value # range: [-1, 1], where 1 means maximum positive output, + # 0 to center servos or minimum motor thrust, + # -1 maximum negative (if not supported by the motors, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) +uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/airspeed.msg b/msg/airspeed.msg index e9b771e638af..9dd5f18ae8fa 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,9 +1,10 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample -float32 indicated_airspeed_m_s # indicated airspeed in m/s +float32 indicated_airspeed_m_s # indicated airspeed in m/s -float32 true_airspeed_m_s # true filtered 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 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown -float32 confidence # confidence value from 0 to 1 for this sensor +float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/airspeed_wind.msg b/msg/airspeed_wind.msg index 03033bbbdd6f..6ca513a35669 100644 --- a/msg/airspeed_wind.msg +++ b/msg/airspeed_wind.msg @@ -9,7 +9,11 @@ float32 variance_east # Wind estimate error variance in east / Y direction (m/s float32 tas_innov # True airspeed innovation float32 tas_innov_var # True airspeed innovation variance -float32 tas_scale # Estimated true airspeed scale factor + +float32 tas_scale_raw # Estimated true airspeed scale factor (not validated) +float32 tas_scale_raw_var # True airspeed scale factor variance + +float32 tas_scale_validated # Estimated true airspeed scale factor after validation float32 beta_innov # Sideslip measurement innovation float32 beta_innov_var # Sideslip measurement innovation variance diff --git a/msg/autotune_attitude_control_status.msg b/msg/autotune_attitude_control_status.msg new file mode 100644 index 000000000000..021a8c345bd1 --- /dev/null +++ b/msg/autotune_attitude_control_status.msg @@ -0,0 +1,35 @@ +uint64 timestamp # time since system start (microseconds) + +float32[5] coeff # coefficients of the identified discrete-time model +float32[5] coeff_var # coefficients' variance of the identified discrete-time model +float32 fitness # fitness of the parameter estimate +float32 innov +float32 dt_model + +float32 kc +float32 ki +float32 kd +float32 kff +float32 att_p + +float32[3] rate_sp + +float32 u_filt +float32 y_filt + +uint8 STATE_IDLE = 0 +uint8 STATE_INIT = 1 +uint8 STATE_ROLL = 2 +uint8 STATE_ROLL_PAUSE = 3 +uint8 STATE_PITCH = 4 +uint8 STATE_PITCH_PAUSE = 5 +uint8 STATE_YAW = 6 +uint8 STATE_YAW_PAUSE = 7 +uint8 STATE_VERIFICATION = 8 +uint8 STATE_APPLY = 9 +uint8 STATE_TEST = 10 +uint8 STATE_COMPLETE = 11 +uint8 STATE_FAIL = 12 +uint8 STATE_WAIT_FOR_DISARM = 13 + +uint8 state diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 29925fe88399..376506d423c6 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -1,15 +1,16 @@ 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 float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown -float32 average_current_a # Battery current average in amperes, -1 if unknown +float32 current_average_a # Battery current average in amperes, -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown float32 scale # Power scaling factor, >= 1, or -1 if unknown +float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown float32 temperature # temperature of the battery. NaN if unknown -int32 cell_count # Number of cells -bool connected # Whether or not a battery is connected, based on a voltage threshold +uint8 cell_count # Number of cells, 0 if unknown uint8 BATTERY_SOURCE_POWER_MODULE = 0 uint8 BATTERY_SOURCE_EXTERNAL = 1 @@ -18,19 +19,19 @@ uint8 source # Battery source uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # actual capacity of the battery uint16 cycle_count # number of discharge cycles the battery has experienced -uint16 run_time_to_empty # predicted remaining battery capacity based on the present rate of discharge in min uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min uint16 serial_number # serial number of the battery pack uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512 -uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity. +uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%. uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100% uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. -uint16 interface_error # SMBUS interface error counter +uint16 interface_error # interface error counter -float32[10] voltage_cell_v # Battery individual cell voltages +float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown float32 max_cell_voltage_delta # Max difference between individual cell voltages bool is_powering_off # Power off event imminent indication, false if unknown +bool is_required # Set if the battery is explicitly required before arming uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active @@ -38,8 +39,40 @@ uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely +uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging + +uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes +uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current +uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one +uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature +uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! -uint8 warning # current battery warning +uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. +uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable. +uint8 warning # Current battery warning +uint8 mode # Battery mode. Note, the normal operation mode + +uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational +uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level) +uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode +uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)! uint8 MAX_INSTANCES = 4 + +float32 average_power # The average power of the current discharge +float32 available_energy # The predicted charge or energy remaining in the battery +float32 full_charge_capacity_wh # The compensated battery capacity +float32 remaining_capacity_wh # The compensated battery capacity remaining +float32 design_capacity # The design capacity of the battery +uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes +uint16 over_discharge_count # Number of battery overdischarge +float32 nominal_voltage # Nominal voltage of the battery pack diff --git a/msg/button_event.msg b/msg/button_event.msg new file mode 100644 index 000000000000..bc745b901727 --- /dev/null +++ b/msg/button_event.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +bool triggered # Set to true if the event is triggered + +# TOPICS button_event safety_button + +uint8 ORB_QUEUE_LENGTH = 2 \ No newline at end of file diff --git a/msg/camera_status.msg b/msg/camera_status.msg new file mode 100644 index 000000000000..c83be897d971 --- /dev/null +++ b/msg/camera_status.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 active_sys_id # mavlink system id of the currently active camera +uint8 active_comp_id # mavlink component id of currently active camera diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg index 6562874c36f9..0da1dd109d3e 100644 --- a/msg/camera_trigger.msg +++ b/msg/camera_trigger.msg @@ -4,4 +4,6 @@ uint64 timestamp_utc # UTC timestamp uint32 seq # Image sequence number bool feedback # Trigger feedback from camera -# TOPICS camera_trigger camera_trigger_secondary \ No newline at end of file +uint32 ORB_QUEUE_LENGTH = 2 + +# TOPICS camera_trigger \ No newline at end of file diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 7d5c85e11d3e..ca5162581964 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -16,8 +16,9 @@ uint8 MAIN_STATE_AUTO_LAND = 11 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_AUTO_PRECLAND = 13 uint8 MAIN_STATE_ORBIT = 14 -uint8 MAIN_STATE_MAX = 15 +uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF = 15 +uint8 MAIN_STATE_MAX = 16 -uint8 main_state # main state machine +uint8 main_state uint16 main_state_changes diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg index 2e90dbd454ce..3fbbe844efbc 100644 --- a/msg/control_allocator_status.msg +++ b/msg/control_allocator_status.msg @@ -19,3 +19,5 @@ int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a valu int8[16] actuator_saturation # Indicates actuator saturation status. # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved. # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value. + +uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg index bfb8f854780d..b561b2271bd0 100644 --- a/msg/differential_pressure.msg +++ b/msg/differential_pressure.msg @@ -1,6 +1,10 @@ -uint64 timestamp # time since system start (microseconds) -uint64 error_count # Number of errors detected by driver -float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative) -float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading -float32 temperature # Temperature provided by sensor, -1000.0f if unknown -uint32 device_id # unique device ID for the sensor that does not change between power cycles +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 differential_pressure_pa # differential pressure reading in Pascals (may be negative) + +float32 temperature # Temperature provided by sensor in celcius, NAN if unknown + +uint32 error_count # Number of errors detected by driver diff --git a/msg/ekf2_timestamps.msg b/msg/ekf2_timestamps.msg index 0b4d1c0c8799..ae3ac067669f 100644 --- a/msg/ekf2_timestamps.msg +++ b/msg/ekf2_timestamps.msg @@ -1,5 +1,5 @@ -# this message contains the (relative) timestamps of the sensor inputs used by -# ekf2. It can be used for reproducible replay. +# this message contains the (relative) timestamps of the sensor inputs used by EKF2. +# It can be used for reproducible replay. # the timestamp field is the ekf2 reference time and matches the timestamp of # the sensor_combined topic. diff --git a/msg/ekf_gps_drift.msg b/msg/ekf_gps_drift.msg deleted file mode 100644 index 5016c97c0e9b..000000000000 --- a/msg/ekf_gps_drift.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -float32 hpos_drift_rate # Horizontal position rate magnitude checked using EKF2_REQ_HDRIFT (m/s) -float32 vpos_drift_rate # Vertical position rate magnitude checked using EKF2_REQ_VDRIFT (m/s) -float32 hspd # Filtered horizontal velocity magnitude checked using EKF2_REQ_HDRIFT (m/s) -bool blocked # true when drift calculation is blocked due to IMU movement check controlled by EKF2_MOVE_TEST diff --git a/msg/esc_report.msg b/msg/esc_report.msg index 508a8b9ebd19..1b184503804f 100644 --- a/msg/esc_report.msg +++ b/msg/esc_report.msg @@ -3,18 +3,23 @@ uint32 esc_errorcount # Number of reported errors by ESC - if supported int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported float32 esc_voltage # Voltage measured from current ESC [V] - if supported float32 esc_current # Current measured from current ESC [A] - if supported -uint8 esc_temperature # Temperature measured from current ESC [degC] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) uint8 esc_state # State of ESC - depend on Vendor -uint8 failures # Bitmask to indicate the internal ESC faults +uint8 actuator_function # actuator output function (one of Motor1...MotorN) -uint8 FAILURE_NONE = 0 -uint8 FAILURE_OVER_CURRENT_MASK = 1 # (1 << 0) -uint8 FAILURE_OVER_VOLTAGE_MASK = 2 # (1 << 1) -uint8 FAILURE_OVER_TEMPERATURE_MASK = 4 # (1 << 2) -uint8 FAILURE_OVER_RPM_MASK = 8 # (1 << 3) -uint8 FAILURE_INCONSISTENT_CMD_MASK = 16 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) -uint8 FAILURE_MOTOR_STUCK_MASK = 32 # (1 << 5) -uint8 FAILURE_GENERIC_MASK = 64 # (1 << 6) +uint16 failures # Bitmask to indicate the internal ESC faults + +uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0) +uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1) +uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2) +uint8 FAILURE_OVER_RPM = 3 # (1 << 3) +uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) +uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5) +uint8 FAILURE_GENERIC = 6 # (1 << 6) +uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7) +uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8) +uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9) +uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element! diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg new file mode 100644 index 000000000000..c0f6695fd4b8 --- /dev/null +++ b/msg/estimator_aid_source_1d.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32 observation +float32 observation_variance + +float32 innovation +float32 innovation_variance +float32 test_ratio + +bool fusion_enabled # true when measurements are being fused +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_source_1d +# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt diff --git a/msg/estimator_aid_source_2d.msg b/msg/estimator_aid_source_2d.msg new file mode 100644 index 000000000000..c70403ff99e0 --- /dev/null +++ b/msg/estimator_aid_source_2d.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64[2] time_last_fuse + +float32[2] observation +float32[2] observation_variance + +float32[2] innovation +float32[2] innovation_variance +float32[2] test_ratio + +bool[2] fusion_enabled # true when measurements are being fused +bool[2] innovation_rejected # true if the observation has been rejected +bool[2] fused # true if the sample was successfully fused + +# TOPICS estimator_aid_source_2d +# TOPICS estimator_aid_src_fake_pos diff --git a/msg/estimator_aid_source_3d.msg b/msg/estimator_aid_source_3d.msg new file mode 100644 index 000000000000..b2d158487d1a --- /dev/null +++ b/msg/estimator_aid_source_3d.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64[3] time_last_fuse + +float32[3] observation +float32[3] observation_variance + +float32[3] innovation +float32[3] innovation_variance +float32[3] test_ratio + +bool[3] fusion_enabled # true when measurements are being fused +bool[3] innovation_rejected # true if the observation has been rejected +bool[3] fused # true if the sample was successfully fused + +# TOPICS estimator_aid_source_3d +# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel diff --git a/msg/estimator_baro_bias.msg b/msg/estimator_baro_bias.msg new file mode 100644 index 000000000000..52b807c610ed --- /dev/null +++ b/msg/estimator_baro_bias.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 baro_device_id # unique device ID for the sensor that does not change between power cycles +float32 bias # estimated barometric altitude bias (m) +float32 bias_var # estimated barometric altitude bias variance (m^2) + +float32 innov # innovation of the last measurement fusion (m) +float32 innov_var # innovation variance of the last measurement fusion (m^2) +float32 innov_test_ratio # normalized innovation squared test ratio diff --git a/msg/estimator_event_flags.msg b/msg/estimator_event_flags.msg index 677fd9e60c3f..62efa0a0da9e 100644 --- a/msg/estimator_event_flags.msg +++ b/msg/estimator_event_flags.msg @@ -16,6 +16,10 @@ bool starting_vision_pos_fusion # 9 - true when the filter starts using bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data +bool reset_hgt_to_baro # 13 - true when the vertical position state is reset to the baro measurement +bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement +bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement +bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement # warning events uint32 warning_event_changes # number of warning event changes @@ -30,3 +34,4 @@ bool bad_yaw_using_gps_course # 7 - true when the fiter has detected bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetomer data +bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/msg/estimator_gps_status.msg b/msg/estimator_gps_status.msg new file mode 100644 index 000000000000..2d2462ee5c30 --- /dev/null +++ b/msg/estimator_gps_status.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool checks_passed + +bool check_fail_gps_fix # 0 : insufficient fix type (no 3D solution) +bool check_fail_min_sat_count # 1 : minimum required sat count fail +bool check_fail_max_pdop # 2 : maximum allowed PDOP fail +bool check_fail_max_horz_err # 3 : maximum allowed horizontal position error fail +bool check_fail_max_vert_err # 4 : maximum allowed vertical position error fail +bool check_fail_max_spd_err # 5 : maximum allowed speed error fail +bool check_fail_max_horz_drift # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +bool check_fail_max_vert_drift # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail + +float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s) +float32 position_drift_rate_vertical_m_s # Vertical position rate magnitude (m/s) +float32 filtered_horizontal_speed_m_s # Filtered horizontal velocity magnitude (m/s) diff --git a/msg/estimator_innovations.msg b/msg/estimator_innovations.msg index cb3e85a1c45f..56a741b37d9a 100644 --- a/msg/estimator_innovations.msg +++ b/msg/estimator_innovations.msg @@ -31,6 +31,7 @@ float32[2] drag # drag specific force innovation (m/sec**2) and innovation vari float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) float32 hagl # height of ground innovation (m) and innovation variance (m**2) +float32 hagl_rate # height of ground rate innovation (m/s) and innovation variance ((m/s)**2) # The innovation test ratios are scalar values. In case the field is a vector, # the test ratio will be put in the first component of the vector. diff --git a/msg/estimator_optical_flow_vel.msg b/msg/estimator_optical_flow_vel.msg deleted file mode 100644 index 38ee0d7f6845..000000000000 --- a/msg/estimator_optical_flow_vel.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) - -float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) -float32[2] vel_ne # same as vel_body but in local frame (m/s) -float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) -float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) -float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index 6999bea7a4d6..f42e1aa87cee 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -13,15 +13,18 @@ float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s) float32[3] gyro_bias_variance bool gyro_bias_valid +bool gyro_bias_stable # true when the gyro bias estimate is stable enough to use for calibration uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2) float32[3] accel_bias_variance bool accel_bias_valid +bool accel_bias_stable # true when the accel bias estimate is stable enough to use for calibration uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss) float32[3] mag_bias_variance bool mag_bias_valid +bool mag_bias_stable # true when the mag bias estimate is stable enough to use for calibration diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 7f8dc28cb074..efbb3ed43c66 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -1,11 +1,6 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[3] vibe # IMU vibration metrics in the following array locations -# 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) -# 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) -# 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) - float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below @@ -46,6 +41,11 @@ uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect ind uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed +uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from external vision measurements is intended +uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest +uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used +uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used uint32 filter_fault_flags # Bitmask to indicate EKF internal faults # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error diff --git a/msg/estimator_status_flags.msg b/msg/estimator_status_flags.msg index 2e9c73711922..5176db9af59d 100644 --- a/msg/estimator_status_flags.msg +++ b/msg/estimator_status_flags.msg @@ -31,7 +31,11 @@ bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest - +bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used +bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used +bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift +bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements +bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/event.msg b/msg/event.msg new file mode 100644 index 000000000000..5f1fc7ef08a1 --- /dev/null +++ b/msg/event.msg @@ -0,0 +1,10 @@ +# Events interface +uint64 timestamp # time since system start (microseconds) + +uint32 id # Event ID +uint16 event_sequence # Event sequence number +uint8[25] arguments # (optional) arguments, depend on event id + +uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/failure_detector_status.msg b/msg/failure_detector_status.msg new file mode 100644 index 000000000000..923ceb36da4f --- /dev/null +++ b/msg/failure_detector_status.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) + +# FailureDetector status +bool fd_roll +bool fd_pitch +bool fd_alt +bool fd_ext +bool fd_arm_escs +bool fd_battery +bool fd_imbalanced_prop +bool fd_motor + +float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) +uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures diff --git a/msg/follow_target.msg b/msg/follow_target.msg index f0afe3f4b86e..e88c2460c8c0 100644 --- a/msg/follow_target.msg +++ b/msg/follow_target.msg @@ -1,8 +1,11 @@ -uint64 timestamp # time since system start (microseconds) -float64 lat # target position (deg * 1e7) -float64 lon # target position (deg * 1e7) -float32 alt # target position -float32 vy # target vel in y -float32 vx # target vel in x -float32 vz # target vel in z -uint8 est_cap # target reporting capabilities +uint64 timestamp # time since system start (microseconds) + +float64 lat # target position (deg * 1e7) +float64 lon # target position (deg * 1e7) +float32 alt # target position + +float32 vy # target vel in y +float32 vx # target vel in x +float32 vz # target vel in z + +uint8 est_cap # target reporting capabilities diff --git a/msg/follow_target_estimator.msg b/msg/follow_target_estimator.msg new file mode 100644 index 000000000000..9d3df9f6f880 --- /dev/null +++ b/msg/follow_target_estimator.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 last_filter_reset_timestamp # time of last filter reset (microseconds) + +bool valid # True if estimator states are okay to be used +bool stale # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate. + +float64 lat_est # Estimated target latitude +float64 lon_est # Estimated target longitude +float32 alt_est # Estimated target altitude + +float32[3] pos_est # Estimated target NED position (m) +float32[3] vel_est # Estimated target NED velocity (m/s) +float32[3] acc_est # Estimated target NED acceleration (m^2/s) + +uint64 prediction_count +uint64 fusion_count diff --git a/msg/follow_target_status.msg b/msg/follow_target_status.msg new file mode 100644 index 000000000000..713a7dcdbd2c --- /dev/null +++ b/msg/follow_target_status.msg @@ -0,0 +1,12 @@ +uint64 timestamp # [microseconds] time since system start + +float32 tracked_target_course # [rad] Tracked target course in NED local frame (North is course zero) +float32 follow_angle # [rad] Current follow angle setting + +float32 orbit_angle_setpoint # [rad] Current orbit angle setpoint from the smooth trajectory generator +float32 angular_rate_setpoint # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle + +float32[3] desired_position_raw # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places + +bool in_emergency_ascent # [bool] True when doing emergency ascent (when distance to ground is below safety altitude) +float32 gimbal_pitch # [rad] Gimbal pitch commanded to track target in the center of the frame diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index fcae7a28c471..746eadfd7b2f 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -1,12 +1,12 @@ -uint64 timestamp # time since system start (microseconds) -uint8 GF_ACTION_NONE = 0 # no action on geofence violation -uint8 GF_ACTION_WARN = 1 # critical mavlink message -uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER -uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL -uint8 GF_ACTION_TERMINATE = 4 # flight termination -uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND +uint64 timestamp # time since system start (microseconds) +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND -bool geofence_violated # true if the geofence is violated -uint8 geofence_action # action to take when geofence is violated +bool geofence_violated # true if the geofence is violated +uint8 geofence_action # action to take when geofence is violated -bool home_required # true if the geofence requires a valid home position +bool home_required # true if the geofence requires a valid home position diff --git a/msg/gimbal_device_attitude_status.msg b/msg/gimbal_device_attitude_status.msg index de2d7e892b37..0be66babe121 100644 --- a/msg/gimbal_device_attitude_status.msg +++ b/msg/gimbal_device_attitude_status.msg @@ -16,3 +16,5 @@ float32 angular_velocity_y float32 angular_velocity_z uint32 failure_flags + +bool received_from_mavlink diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg index 66d431143b14..1cdaca85ea07 100644 --- a/msg/gps_inject_data.msg +++ b/msg/gps_inject_data.msg @@ -1,6 +1,9 @@ uint64 timestamp # time since system start (microseconds) -uint8 len # length of data -uint8 flags # LSB: 1=fragmented -uint8[182] data # data to write to GPS device (RTCM message) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint8 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[300] data # data to write to GPS device (RTCM message) uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/heater_status.msg b/msg/heater_status.msg index 158d28fb992d..44207a3989c5 100644 --- a/msg/heater_status.msg +++ b/msg/heater_status.msg @@ -3,6 +3,7 @@ uint64 timestamp # time since system start (microseconds) uint32 device_id bool heater_on +bool temperature_target_met float32 temperature_sensor float32 temperature_target diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 8f333e7f66d2..d26c31ea0f9f 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -23,6 +23,7 @@ uint64 timestamp_last_signal # last valid reception time uint8 channel_count # number of channels actually being seen +int8 RSSI_MAX = 100 int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. diff --git a/msg/internal_combustion_engine_status.msg b/msg/internal_combustion_engine_status.msg new file mode 100644 index 000000000000..301eb92a861d --- /dev/null +++ b/msg/internal_combustion_engine_status.msg @@ -0,0 +1,64 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_STOPPED = 0 # The engine is not running. This is the default state. +uint8 STATE_STARTING = 1 # The engine is starting. This is a transient state. +uint8 STATE_RUNNING = 2 # The engine is running normally. +uint8 STATE_FAULT = 3 # The engine can no longer function. +uint8 state + +uint32 FLAG_GENERAL_ERROR = 1 # General error. + +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED = 2 # Error of the crankshaft sensor. This flag is optional. +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR = 4 + +uint32 FLAG_TEMPERATURE_SUPPORTED = 8 # Temperature levels. These flags are optional +uint32 FLAG_TEMPERATURE_BELOW_NOMINAL = 16 # Under-temperature warning +uint32 FLAG_TEMPERATURE_ABOVE_NOMINAL = 32 # Over-temperature warning +uint32 FLAG_TEMPERATURE_OVERHEATING = 64 # Critical overheating +uint32 FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL = 128 # Exhaust gas over-temperature warning + +uint32 FLAG_FUEL_PRESSURE_SUPPORTED = 256 # Fuel pressure. These flags are optional +uint32 FLAG_FUEL_PRESSURE_BELOW_NOMINAL = 512 # Under-pressure warning +uint32 FLAG_FUEL_PRESSURE_ABOVE_NOMINAL = 1024 # Over-pressure warning + +uint32 FLAG_DETONATION_SUPPORTED = 2048 # Detonation warning. This flag is optional. +uint32 FLAG_DETONATION_OBSERVED = 4096 # Detonation condition observed warning + +uint32 FLAG_MISFIRE_SUPPORTED = 8192 # Misfire warning. This flag is optional. +uint32 FLAG_MISFIRE_OBSERVED = 16384 # Misfire condition observed warning + +uint32 FLAG_OIL_PRESSURE_SUPPORTED = 32768 # Oil pressure. These flags are optional +uint32 FLAG_OIL_PRESSURE_BELOW_NOMINAL = 65536 # Under-pressure warning +uint32 FLAG_OIL_PRESSURE_ABOVE_NOMINAL = 131072 # Over-pressure warning + +uint32 FLAG_DEBRIS_SUPPORTED = 262144 # Debris warning. This flag is optional +uint32 FLAG_DEBRIS_DETECTED = 524288 # Detection of debris warning +uint32 flags + +uint8 engine_load_percent # Engine load estimate, percent, [0, 127] +uint32 engine_speed_rpm # Engine speed, revolutions per minute +float32 spark_dwell_time_ms # Spark dwell time, millisecond +float32 atmospheric_pressure_kpa # Atmospheric (barometric) pressure, kilopascal +float32 intake_manifold_pressure_kpa # Engine intake manifold pressure, kilopascal +float32 intake_manifold_temperature # Engine intake manifold temperature, kelvin +float32 coolant_temperature # Engine coolant temperature, kelvin +float32 oil_pressure # Oil pressure, kilopascal +float32 oil_temperature # Oil temperature, kelvin +float32 fuel_pressure # Fuel pressure, kilopascal +float32 fuel_consumption_rate_cm3pm # Instant fuel consumption estimate, (centimeter^3)/minute +float32 estimated_consumed_fuel_volume_cm3 # Estimate of the consumed fuel since the start of the engine, centimeter^3 +uint8 throttle_position_percent # Throttle position, percent +uint8 ecu_index # The index of the publishing ECU + + +uint8 SPARK_PLUG_SINGLE = 0 +uint8 SPARK_PLUG_FIRST_ACTIVE = 1 +uint8 SPARK_PLUG_SECOND_ACTIVE = 2 +uint8 SPARK_PLUG_BOTH_ACTIVE = 3 +uint8 spark_plug_usage # Spark plug activity report. + +float32 ignition_timing_deg # Cylinder ignition timing, angular degrees of the crankshaft +float32 injection_time_ms # Fuel injection time, millisecond +float32 cylinder_head_temperature # Cylinder head temperature (CHT), kelvin +float32 exhaust_gas_temperature # Exhaust gas temperature (EGT), kelvin +float32 lambda_coefficient # Estimated lambda coefficient, dimensionless ratio diff --git a/msg/magnetometer_bias_estimate.msg b/msg/magnetometer_bias_estimate.msg new file mode 100644 index 000000000000..3c0c1136cf4a --- /dev/null +++ b/msg/magnetometer_bias_estimate.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32[4] bias_x # estimated X-bias of all the sensors +float32[4] bias_y # estimated Y-bias of all the sensors +float32[4] bias_z # estimated Z-bias of all the sensors + +bool[4] valid # true if the estimator has converged +bool[4] stable diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 16abfedb2609..bbbdd0bc6660 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -1,14 +1,18 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +bool valid -uint8 SOURCE_RC = 1 # radio control +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 -uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 4 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 -uint8 data_source # where this input is coming from +uint8 data_source # Any of the channels may not be available and be set to NaN # to indicate that it does not contain valid data. @@ -40,10 +44,13 @@ float32 r # yaw stick/twist position, -1..1 float32 flaps # flap position -float32 aux1 # default function: camera yaw / azimuth -float32 aux2 # default function: camera pitch / tilt -float32 aux3 # default function: camera trigger -float32 aux4 # default function: camera roll -float32 aux5 # default function: payload drop +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 float32 aux6 +bool sticks_moving + +# TOPICS manual_control_setpoint manual_control_input diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg index 444120e5954a..d343a3ab521e 100644 --- a/msg/manual_control_switches.msg +++ b/msg/manual_control_switches.msg @@ -26,11 +26,7 @@ uint8 kill_switch # throttle kill: _NORMAL_, KILL uint8 gear_switch # landing gear switch: _DOWN_, UP uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT -# legacy "advanced" switch configuration (will be removed soon) -uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO -uint8 man_switch # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL -uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 stab_switch # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED -uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL +uint8 photo_switch # Photo trigger switch +uint8 video_switch # Photo trigger switch uint32 switch_changes # number of switch changes diff --git a/msg/mavlink_tunnel.msg b/msg/mavlink_tunnel.msg new file mode 100644 index 000000000000..16934a95226d --- /dev/null +++ b/msg/mavlink_tunnel.msg @@ -0,0 +1,20 @@ +# MAV_TUNNEL_PAYLOAD_TYPE enum + +uint8 MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller + +uint64 timestamp # Time since system start (microseconds) +uint16 payload_type # A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. +uint8 target_system # System ID (can be 0 for broadcast, but this is discouraged) +uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) +uint8 payload_length # Length of the data transported in payload +uint8[128] payload # Data itself diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg deleted file mode 100644 index 2cd692fd363e..000000000000 --- a/msg/multirotor_motor_limits.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated - -# 0 - True if the saturation status is valid -# 1 - True if any motor is saturated at the upper limit -# 2 - True if any motor is saturated at the lower limit -# 3 - True if a positive roll increment will increase motor saturation -# 4 - True if negative roll increment will increase motor saturation -# 5 - True if positive pitch increment will increase motor saturation -# 6 - True if negative pitch increment will increase motor saturation -# 7 - True if positive yaw increment will increase motor saturation -# 8 - True if negative yaw increment will increase motor saturation -# 9 - True if positive thrust increment will increase motor saturation -# 10 - True if negative thrust increment will increase motor saturation diff --git a/msg/npfg_status.msg b/msg/npfg_status.msg new file mode 100644 index 000000000000..ef5538b55137 --- /dev/null +++ b/msg/npfg_status.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 wind_est_valid # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid) +float32 lat_accel # resultant lateral acceleration reference [m/s^2] +float32 lat_accel_ff # lateral acceleration demand only for maintaining curvature [m/s^2] +float32 bearing_feas # bearing feasibility [0,1] +float32 bearing_feas_on_track # on-track bearing feasibility [0,1] +float32 signed_track_error # signed track error [m] +float32 track_error_bound # track error bound [m] +float32 airspeed_ref # (true) airspeed reference [m/s] +float32 bearing # bearing angle [rad] +float32 heading_ref # heading angle reference [rad] +float32 min_ground_speed_ref # minimum forward ground speed reference [m/s] +float32 adapted_period # adapted period (if auto-tuning enabled) [s] +float32 p_gain # controller proportional gain [rad/s] +float32 time_const # controller time constant [s] diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg index 73d4cba08145..319ba1ac5058 100644 --- a/msg/offboard_control_mode.msg +++ b/msg/offboard_control_mode.msg @@ -7,3 +7,4 @@ bool velocity bool acceleration bool attitude bool body_rate +bool actuator diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg deleted file mode 100644 index 4b3796ff9f11..000000000000 --- a/msg/optical_flow.msg +++ /dev/null @@ -1,29 +0,0 @@ -# Optical flow in XYZ body frame in SI units. -# http://en.wikipedia.org/wiki/International_System_of_Units - -uint64 timestamp # time since system start (microseconds) - -uint8 sensor_id # id of the sensor emitting the flow value -float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis -float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis -float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 ground_distance_m # Altitude / distance to ground in meters -uint32 integration_timespan # accumulation timespan in microseconds -uint32 time_since_last_sonar_update # time since last sonar update in microseconds -uint16 frame_count_since_last_readout # number of accumulated frames in timespan -int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius -uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality - -float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably -float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably -float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably - - -uint8 MODE_UNKNOWN = 0 -uint8 MODE_BRIGHT = 1 -uint8 MODE_LOWLIGHT = 2 -uint8 MODE_SUPER_LOWLIGHT = 3 - -uint8 mode diff --git a/msg/pps_capture.msg b/msg/pps_capture.msg new file mode 100644 index 000000000000..b9ec949a9c46 --- /dev/null +++ b/msg/pps_capture.msg @@ -0,0 +1,2 @@ +uint64 timestamp # time since system start (microseconds) at PPS capture event +uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event \ No newline at end of file diff --git a/msg/px4io_status.msg b/msg/px4io_status.msg index 8d830b21f4c7..295c8fba66dc 100644 --- a/msg/px4io_status.msg +++ b/msg/px4io_status.msg @@ -6,49 +6,38 @@ float32 voltage_v # Servo rail voltage in volts float32 rssi_v # RSSI pin voltage in volts # PX4IO status flags (PX4IO_P_STATUS_FLAGS) +bool status_arm_sync +bool status_failsafe +bool status_fmu_initialized +bool status_fmu_ok +bool status_init_ok bool status_outputs_armed -bool status_override +bool status_raw_pwm bool status_rc_ok -bool status_rc_ppm bool status_rc_dsm +bool status_rc_ppm bool status_rc_sbus -bool status_fmu_ok -bool status_raw_pwm -bool status_mixer_ok -bool status_arm_sync -bool status_init_ok -bool status_failsafe -bool status_safety_off -bool status_fmu_initialized bool status_rc_st24 bool status_rc_sumd +bool status_safety_button_event # px4io safety button was pressed for longer than 1 second # PX4IO alarms (PX4IO_P_STATUS_ALARMS) -bool alarm_vbatt_low -bool alarm_temperature -bool alarm_servo_current -bool alarm_acc_current -bool alarm_fmu_lost -bool alarm_rc_lost bool alarm_pwm_error -bool alarm_vservo_fault +bool alarm_rc_lost # PX4IO arming (PX4IO_P_SETUP_ARMING) -bool arming_io_arm_ok +bool arming_failsafe_custom bool arming_fmu_armed bool arming_fmu_prearmed -bool arming_manual_override_ok -bool arming_failsafe_custom -bool arming_inair_restart_ok -bool arming_always_pwm_enable -bool arming_rc_handling_disabled -bool arming_lockdown bool arming_force_failsafe +bool arming_io_arm_ok +bool arming_lockdown bool arming_termination_failsafe -bool arming_override_immediate +uint16[8] pwm +uint16[8] pwm_disarmed +uint16[8] pwm_failsafe -int16[8] actuators -uint16[8] servos +uint16[8] pwm_rate_hz uint16[18] raw_inputs diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg deleted file mode 100644 index d472e8dd33b6..000000000000 --- a/msg/qshell_req.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -char[100] cmd -uint32 MAX_STRLEN = 100 -uint32 strlen -uint32 request_sequence diff --git a/msg/qshell_retval.msg b/msg/qshell_retval.msg deleted file mode 100644 index d42a7715ffe7..000000000000 --- a/msg/qshell_retval.msg +++ /dev/null @@ -1,3 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -int32 return_value -uint32 return_sequence diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 024b5d5deb74..e44812d70da8 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,36 +1,39 @@ uint64 timestamp # time since system start (microseconds) -uint8 FUNCTION_THROTTLE = 0 -uint8 FUNCTION_ROLL = 1 -uint8 FUNCTION_PITCH = 2 -uint8 FUNCTION_YAW = 3 -uint8 FUNCTION_MODE = 4 -uint8 FUNCTION_RETURN = 5 -uint8 FUNCTION_POSCTL = 6 -uint8 FUNCTION_LOITER = 7 -uint8 FUNCTION_OFFBOARD = 8 -uint8 FUNCTION_ACRO = 9 -uint8 FUNCTION_FLAPS = 10 -uint8 FUNCTION_AUX_1 = 11 -uint8 FUNCTION_AUX_2 = 12 -uint8 FUNCTION_AUX_3 = 13 -uint8 FUNCTION_AUX_4 = 14 -uint8 FUNCTION_AUX_5 = 15 -uint8 FUNCTION_PARAM_1 = 16 -uint8 FUNCTION_PARAM_2 = 17 -uint8 FUNCTION_PARAM_3_5 = 18 -uint8 FUNCTION_KILLSWITCH = 19 -uint8 FUNCTION_TRANSITION = 20 -uint8 FUNCTION_GEAR = 21 -uint8 FUNCTION_ARMSWITCH = 22 -uint8 FUNCTION_STAB = 23 -uint8 FUNCTION_AUX_6 = 24 -uint8 FUNCTION_MAN = 25 +uint8 FUNCTION_THROTTLE = 0 +uint8 FUNCTION_ROLL = 1 +uint8 FUNCTION_PITCH = 2 +uint8 FUNCTION_YAW = 3 +uint8 FUNCTION_RETURN = 4 +uint8 FUNCTION_LOITER = 5 +uint8 FUNCTION_OFFBOARD = 6 +uint8 FUNCTION_FLAPS = 7 +uint8 FUNCTION_AUX_1 = 8 +uint8 FUNCTION_AUX_2 = 9 +uint8 FUNCTION_AUX_3 = 10 +uint8 FUNCTION_AUX_4 = 11 +uint8 FUNCTION_AUX_5 = 12 +uint8 FUNCTION_AUX_6 = 13 +uint8 FUNCTION_PARAM_1 = 14 +uint8 FUNCTION_PARAM_2 = 15 +uint8 FUNCTION_PARAM_3_5 = 16 +uint8 FUNCTION_KILLSWITCH = 17 +uint8 FUNCTION_TRANSITION = 18 +uint8 FUNCTION_GEAR = 19 +uint8 FUNCTION_ARMSWITCH = 20 +uint8 FUNCTION_FLTBTN_SLOT_1 = 21 +uint8 FUNCTION_FLTBTN_SLOT_2 = 22 +uint8 FUNCTION_FLTBTN_SLOT_3 = 23 +uint8 FUNCTION_FLTBTN_SLOT_4 = 24 +uint8 FUNCTION_FLTBTN_SLOT_5 = 25 +uint8 FUNCTION_FLTBTN_SLOT_6 = 26 + +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[26] function # Functions mapping +int8[27] 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/rtl_flight_time.msg b/msg/rtl_flight_time.msg deleted file mode 100644 index 1f16b6740898..000000000000 --- a/msg/rtl_flight_time.msg +++ /dev/null @@ -1,4 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 rtl_time_s # how long in seconds will the RTL take -float32 rtl_limit_fraction # what fraction of the allowable RTL time would be taken diff --git a/msg/rtl_time_estimate.msg b/msg/rtl_time_estimate.msg new file mode 100644 index 000000000000..ee46888dd49c --- /dev/null +++ b/msg/rtl_time_estimate.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool valid # Flag indicating whether the time estiamtes are valid +float32 time_estimate # [s] Estimated time for RTL +float32 safe_time_estimate # [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin) diff --git a/msg/safety.msg b/msg/safety.msg deleted file mode 100644 index 62046c71b7f1..000000000000 --- a/msg/safety.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -bool safety_switch_available # Set to true if a safety switch is connected -bool safety_off # Set to true if safety is off -bool override_available # Set to true if external override system is connected -bool override_enabled # Set to true if override is engaged diff --git a/msg/sensor_baro.msg b/msg/sensor_baro.msg index ddbc13d8ef9e..7c4154e159d7 100644 --- a/msg/sensor_baro.msg +++ b/msg/sensor_baro.msg @@ -3,8 +3,10 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -uint32 error_count +float32 pressure # static pressure measurement in Pascals + +float32 temperature # temperature in degrees Celsius -float32 pressure # static pressure measurement in millibar +uint32 error_count -float32 temperature # static temperature measurement in deg Celsius +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 42440213cf57..837c7a1fac1d 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -2,19 +2,24 @@ # These fields are scaled and offset-compensated where possible and do not # change with board revisions and sensor updates. -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid # gyro timstamp is equal to the timestamp of the message -float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period -uint32 gyro_integral_dt # gyro measurement sampling period in microseconds +float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in microseconds -int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp -float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period -uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 -uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period + +uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame +uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/msg/sensor_correction.msg b/msg/sensor_correction.msg index 528ba32ed68c..45ee23135d5e 100644 --- a/msg/sensor_correction.msg +++ b/msg/sensor_correction.msg @@ -7,6 +7,7 @@ uint64 timestamp # time since system start (microseconds) # Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] gyro_device_ids +float32[4] gyro_temperature float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s @@ -15,6 +16,7 @@ float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s # Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] accel_device_ids +float32[4] accel_temperature float32[3] accel_offset_0 # accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s float32[3] accel_offset_1 # accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s float32[3] accel_offset_2 # accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s @@ -23,6 +25,7 @@ float32[3] accel_offset_3 # accelerometer 3 offsets in the FRD board frame XYZ-a # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame uint32[4] baro_device_ids +float32[4] baro_temperature float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in Pascals float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in Pascals float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in Pascals diff --git a/msg/sensor_gnss_relative.msg b/msg/sensor_gnss_relative.msg new file mode 100644 index 000000000000..0f1736f45fd8 --- /dev/null +++ b/msg/sensor_gnss_relative.msg @@ -0,0 +1,30 @@ +# GNSS relative positioning information in NED frame. The NED frame is defined as the local topological system at the reference station. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint16 reference_station_id # Reference Station ID + +float32[3] position # GPS NED relative position vector (m) +float32[3] position_accuracy # Accuracy of relative position (m) + +float32 heading # Heading of the relative position vector (radians) +float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) + +float32 position_length +float32 accuracy_length + +bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) +bool differential_solution # differential corrections were applied +bool relative_position_valid +bool carrier_solution_floating # carrier phase range solution with floating ambiguities +bool carrier_solution_fixed # carrier phase range solution with fixed ambiguities +bool moving_base_mode # if the receiver is operating in moving base mode +bool reference_position_miss # extrapolated reference position was used to compute moving base solution this epoch +bool reference_observations_miss # extrapolated reference observations were used to compute moving base solution this epoch +bool heading_valid +bool relative_position_normalized # the components of the relative position vector (including the high-precision parts) are normalized diff --git a/msg/sensor_gps.msg b/msg/sensor_gps.msg index 2b9519acc52f..a02300df03a9 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -2,6 +2,8 @@ # the field 'timestamp' is for the position & velocity (microseconds) uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles + int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) @@ -36,3 +38,4 @@ uint8 satellites_used # Number of satellites used float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) +float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index b1025e1d12f9..b906127b5814 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -11,6 +11,8 @@ float32 temperature # temperature in degrees Celsius uint32 error_count +uint8[3] clip_counter # clip count per axis in the sample period + uint8 samples # number of raw samples that went into this message uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/sensor_gyro_fft.msg b/msg/sensor_gyro_fft.msg index 66c97e7995a9..ed84d0a0d018 100644 --- a/msg/sensor_gyro_fft.msg +++ b/msg/sensor_gyro_fft.msg @@ -6,10 +6,10 @@ uint32 device_id # unique device ID for the sensor that does not change float32 sensor_sample_rate_hz float32 resolution_hz -float32[6] peak_frequencies_x # x axis peak frequencies -float32[6] peak_frequencies_y # y axis peak frequencies -float32[6] peak_frequencies_z # z axis peak frequencies +float32[3] peak_frequencies_x # x axis peak frequencies +float32[3] peak_frequencies_y # y axis peak frequencies +float32[3] peak_frequencies_z # z axis peak frequencies -uint32[6] peak_magnitude_x # x axis peak frequencies magnitude -uint32[6] peak_magnitude_y # y axis peak frequencies magnitude -uint32[6] peak_magnitude_z # z axis peak frequencies magnitude +float32[3] peak_snr_x # x axis peak SNR +float32[3] peak_snr_y # y axis peak SNR +float32[3] peak_snr_z # z axis peak SNR diff --git a/msg/sensor_hygrometer.msg b/msg/sensor_hygrometer.msg new file mode 100755 index 000000000000..400f560bd6d4 --- /dev/null +++ b/msg/sensor_hygrometer.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 temperature # Temperature provided by sensor (Celcius) + +float32 humidity # Humidity provided by sensor diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg index 438a11b2f010..1b5ba487edbc 100644 --- a/msg/sensor_mag.msg +++ b/msg/sensor_mag.msg @@ -11,6 +11,4 @@ float32 temperature # temperature in degrees Celsius uint32 error_count -bool is_external # if true the mag is external (i.e. not built into the board) - uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/sensor_optical_flow.msg b/msg/sensor_optical_flow.msg new file mode 100644 index 000000000000..ce7e8bf08c81 --- /dev/null +++ b/msg/sensor_optical_flow.msg @@ -0,0 +1,30 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +bool delta_angle_available + +float32 distance_m # (meters) Distance to the center of the flow field +bool distance_available + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # quality, 0: bad quality, 255: maximum quality + +uint32 error_count + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/msg/sensors_status.msg b/msg/sensors_status.msg new file mode 100644 index 000000000000..28c279781212 --- /dev/null +++ b/msg/sensors_status.msg @@ -0,0 +1,15 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32 device_id_primary # current primary device id for reference + +uint32[4] device_ids +float32[4] inconsistency # magnitude of difference between sensor instance and mean +bool[4] healthy # sensor healthy +uint8[4] priority +bool[4] enabled +bool[4] external + +# TOPICS sensors_status sensors_status_baro sensors_status_mag diff --git a/msg/sensors_status_imu.msg b/msg/sensors_status_imu.msg index 29bc7b1ad1f0..cfad3419c33e 100644 --- a/msg/sensors_status_imu.msg +++ b/msg/sensors_status_imu.msg @@ -8,10 +8,11 @@ uint32 accel_device_id_primary # current primary accel device id for refer uint32[4] accel_device_ids float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in m/s^2. bool[4] accel_healthy - +uint8[4] accel_priority uint32 gyro_device_id_primary # current primary gyro device id for reference uint32[4] gyro_device_ids float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s). bool[4] gyro_healthy +uint8[4] gyro_priority diff --git a/msg/takeoff_status.msg b/msg/takeoff_status.msg index 9b55b08f4f53..ed2f906d77ca 100644 --- a/msg/takeoff_status.msg +++ b/msg/takeoff_status.msg @@ -10,3 +10,5 @@ uint8 TAKEOFF_STATE_RAMPUP = 4 uint8 TAKEOFF_STATE_FLIGHT = 5 uint8 takeoff_state + +float32 tilt_limit # limited tilt feasability during takeoff, contains maximum tilt otherwise diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index 7e4fe08228a4..74c47e1a2c90 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -17,6 +17,8 @@ float32 true_airspeed_sp float32 true_airspeed_filtered float32 true_airspeed_derivative_sp float32 true_airspeed_derivative +float32 true_airspeed_derivative_raw +float32 true_airspeed_innovation float32 total_energy_error float32 energy_distribution_error @@ -37,5 +39,6 @@ float32 throttle_integ float32 pitch_integ float32 throttle_sp +float32 pitch_sp_rad uint8 mode diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index e7c3a695091d..74a08efe91d2 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -28,7 +28,6 @@ uint32 tx_buffer_overruns # number of TX buffer overruns float32 rx_rate_avg # transmit rate average (Bytes/s) uint32 rx_message_count # count of total messages received -uint32 rx_message_count_supported # count of total messages received from supported systems and components (for loss statistics) uint32 rx_message_lost_count uint32 rx_buffer_overruns # number of RX buffer overruns uint32 rx_parse_errors # number of parse errors @@ -36,7 +35,7 @@ uint32 rx_packet_drop_count # number of packet drops float32 rx_message_lost_rate -uint64 HEARTBEAT_TIMEOUT_US = 1500000 # Heartbeat timeout 1.5 seconds +uint64 HEARTBEAT_TIMEOUT_US = 2500000 # Heartbeat timeout (tolerate missing 1 + jitter) # Heartbeats per type bool heartbeat_type_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER @@ -45,6 +44,7 @@ bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL bool heartbeat_type_adsb # MAV_TYPE_ADSB bool heartbeat_type_camera # MAV_TYPE_CAMERA +bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE # Heartbeats per component bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO @@ -58,3 +58,4 @@ bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health bool avoidance_system_healthy +bool parachute_system_healthy diff --git a/msg/templates/module.yaml b/msg/templates/module.yaml new file mode 100644 index 000000000000..099976c11e2a --- /dev/null +++ b/msg/templates/module.yaml @@ -0,0 +1,6 @@ +module_name: Sagtech MXS +serial_config: + - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} + port_config_param: + name: TRANS_MXS_CFG + group: Transponders diff --git a/msg/templates/px4/ros/msg.h.em b/msg/templates/px4/ros/msg.h.em index 3264bbaede21..e102a087d4e7 100644 --- a/msg/templates/px4/ros/msg.h.em +++ b/msg/templates/px4/ros/msg.h.em @@ -14,8 +14,6 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * diff --git a/msg/templates/px4/uorb/msg.h.em b/msg/templates/px4/uorb/msg.h.em index 4b848ce361f5..deed11912d4c 100644 --- a/msg/templates/px4/uorb/msg.h.em +++ b/msg/templates/px4/uorb/msg.h.em @@ -14,8 +14,6 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * diff --git a/msg/templates/ucdr/msg.h.em b/msg/templates/ucdr/msg.h.em new file mode 100644 index 000000000000..589533631e82 --- /dev/null +++ b/msg/templates/ucdr/msg.h.em @@ -0,0 +1,128 @@ +@############################################### +@# +@# EmPy template +@# +@############################################### +@# generates CDR serialization & deserialization methods +@# +@# Context: +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - search_path (str) Path to .msg files +@############################################### +@{ +import genmsg.msgs +from px_generate_uorb_topic_helper import * # this is in Tools/ + +topic = spec.short_name +uorb_struct = '%s_s'%spec.short_name + +# get fields, struct size and paddings +def add_fields(msg_fields, name_prefix='', offset=0): + fields = [] + for field in msg_fields: + if not field.is_header: + field_size = sizeof_field_type(field) + + type_name = field.type + # detect embedded types + sl_pos = type_name.find('/') + if (sl_pos >= 0): + package = type_name[:sl_pos] + type_name = type_name[sl_pos + 1:] + + # detect arrays + a_pos = type_name.find('[') + array_size = 1 + if (a_pos >= 0): + # field is array + array_size = int(type_name[a_pos+1:-1]) + type_name = type_name[:a_pos] + + if sl_pos >= 0: # nested type + + children_fields = get_children_fields(field.base_type, search_path) + + for i in range(array_size): + sub_name_prefix = name_prefix+field.name + if array_size > 1: + sub_name_prefix += '['+str(i)+']' + sub_fields, offset = add_fields(children_fields, sub_name_prefix+'.', offset) + fields.extend(sub_fields) + else: + assert field_size > 0 + + # note: the maximum alignment for XCDR is 8 and for XCDR2 it is 4 + padding = (field_size - (offset % field_size)) & (field_size - 1) + + fields.append((type_name, name_prefix+field.name, field_size * array_size, padding)) + offset += array_size * field_size + padding + return fields, offset + +fields, struct_size = add_fields(spec.parsed_fields()) + +}@ + +// auto-generated file + +#pragma once + +#include +#include +#include + +@############################## +@# Includes for dependencies +@############################## +@{ +for field in spec.parsed_fields(): + if (not field.is_builtin): + if not field.is_header: + (package, name) = genmsg.names.package_resource_name(field.base_type) + package = package or spec.package # convert '' to package + print('#include '%(name)) +}@ + +static inline constexpr int ucdr_topic_size_@(topic)() +{ + return @(struct_size); +} + +bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf) +{ + if (ucdr_buffer_remaining(&buf) < @(struct_size)) { + return false; + } +@{ +for field_type, field_name, field_size, padding in fields: + if padding > 0: + print('\tbuf.iterator += {:}; // padding'.format(padding)) + print('\tbuf.offset += {:}; // padding'.format(padding)) + + print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) + print('\tmemcpy(buf.iterator, &topic.{0}, sizeof(topic.{0}));'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) + print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) + +}@ + return true; +} + +bool ucdr_deserialize_@(topic)(ucdrBuffer& buf, @(uorb_struct)& topic) +{ + if (ucdr_buffer_remaining(&buf) < @(struct_size)) { + return false; + } +@{ +for field_type, field_name, field_size, padding in fields: + if padding > 0: + print('\tbuf.iterator += {:}; // padding'.format(padding)) + print('\tbuf.offset += {:}; // padding'.format(padding)) + + print('\tstatic_assert(sizeof(topic.{0}) == {1}, "size mismatch");'.format(field_name, field_size)) + print('\tmemcpy(&topic.{0}, buf.iterator, sizeof(topic.{0}));'.format(field_name)) + print('\tbuf.iterator += sizeof(topic.{:});'.format(field_name)) + print('\tbuf.offset += sizeof(topic.{:});'.format(field_name)) + +}@ + return true; +} diff --git a/msg/templates/uorb/msg.cpp.em b/msg/templates/uorb/msg.cpp.em index a462f063fdbc..03776549f565 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/msg/templates/uorb/msg.cpp.em @@ -12,15 +12,13 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification @# - search_path (dict) search paths for genmsg @# - topics (List of String) multi-topic names @# - constrained_flash set to true if flash is constrained -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -63,7 +61,7 @@ topic_name = spec.short_name sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) -topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields] +topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields] }@ #include @@ -84,19 +82,11 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast(ORB_ID::@multi_topic)); @[end for] -void print_message(const @uorb_struct &message) +void print_message(const orb_metadata *meta, const @uorb_struct& message) { -@[if constrained_flash] - (void)message; - PX4_INFO_RAW("Not implemented on flash constrained hardware\n"); -@[else] - PX4_INFO_RAW(" @(uorb_struct)\n"); - - const hrt_abstime now = hrt_absolute_time(); - -@[for field in sorted_fields]@ - @( print_field(field) )@ -@[end for]@ -@[end if]@ - + if (sizeof(message) != meta->o_size) { + printf("unexpected message size for %s: %zu != %i\n", meta->o_name, sizeof(message), meta->o_size); + return; + } + orb_print_message_internal(meta, &message, true); } diff --git a/msg/templates/uorb/msg.h.em b/msg/templates/uorb/msg.h.em index 0180ebcf0dad..9854e1eaef9d 100644 --- a/msg/templates/uorb/msg.h.em +++ b/msg/templates/uorb/msg.h.em @@ -12,14 +12,12 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification @# - search_path (dict) search paths for genmsg @# - topics (List of String) multi-topic names -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -135,5 +133,5 @@ ORB_DECLARE(@multi_topic); @[end for] #ifdef __cplusplus -void print_message(const @uorb_struct& message); +void print_message(const orb_metadata *meta, const @uorb_struct& message); #endif diff --git a/msg/templates/uorb/uORBTopics.cpp.em b/msg/templates/uorb/uORBTopics.cpp.em index bce579b1a463..c71082e354a4 100644 --- a/msg/templates/uorb/uORBTopics.cpp.em +++ b/msg/templates/uorb/uORBTopics.cpp.em @@ -8,12 +8,11 @@ @# @# Context: @# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - topics (List) list of all topic names @############################################### /**************************************************************************** * - * Copyright (C) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 @@ -49,17 +48,17 @@ @{ msg_names = [mn.replace(".msg", "") for mn in msgs] msgs_count = len(msg_names) -msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates -msg_names_all.sort() -msgs_count_all = len(msg_names_all) +topics_all = topics +topics_all.sort() +topics_count_all = len(topics_all) }@ @[for msg_name in msg_names]@ #include @[end for] const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = { -@[for idx, msg_name in enumerate(msg_names_all, 1)]@ - ORB_ID(@(msg_name))@[if idx != msgs_count_all], @[end if] +@[for idx, topic_name in enumerate(topics_all, 1)]@ + ORB_ID(@(topic_name))@[if idx != topics_count_all], @[end if] @[end for] }; diff --git a/msg/templates/uorb/uORBTopics.hpp.em b/msg/templates/uorb/uORBTopics.hpp.em index d1d33f48160f..528b95acee60 100644 --- a/msg/templates/uorb/uORBTopics.hpp.em +++ b/msg/templates/uorb/uORBTopics.hpp.em @@ -8,12 +8,11 @@ @# @# Context: @# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - topics (List) list of all topic names @############################################### /**************************************************************************** * - * 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 @@ -47,9 +46,9 @@ @{ msg_names = [mn.replace(".msg", "") for mn in msgs] msgs_count = len(msg_names) -msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates -msg_names_all.sort() -msgs_count_all = len(msg_names_all) +topics_all = topics +topics_all.sort() +topics_count_all = len(topics_all) }@ #pragma once @@ -58,7 +57,7 @@ msgs_count_all = len(msg_names_all) #include -static constexpr size_t ORB_TOPICS_COUNT{@(msgs_count_all)}; +static constexpr size_t ORB_TOPICS_COUNT{@(topics_count_all)}; static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } /* @@ -67,7 +66,7 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } extern const struct orb_metadata *const *orb_get_topics() __EXPORT; enum class ORB_ID : uint8_t { -@[for idx, msg_name in enumerate(msg_names_all)]@ +@[for idx, msg_name in enumerate(topics_all)]@ @(msg_name) = @(idx), @[end for] INVALID diff --git a/msg/templates/uorb_microcdr/dds_topics.h.em b/msg/templates/uorb_microcdr/dds_topics.h.em new file mode 100644 index 000000000000..8d7b5e7a671d --- /dev/null +++ b/msg/templates/uorb_microcdr/dds_topics.h.em @@ -0,0 +1,225 @@ +@############################################### +@# +@# EmPy template +@# +@############################################### +@# Generates publications and subscriptions for XRCE +@# +@# Context: +@# - msgs (List) list of all RTPS messages +@# - topics (List) list of all topic names +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@############################################### +@{ +import os + +import genmsg.msgs + +from px_generate_uorb_topic_files import MsgScope # this is in Tools/ + +topic_names = [s.short_name for s in spec] +send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] +recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] +}@ + + +#include +#include + +#include +#include +@[for idx, topic in enumerate(send_topics)]@ +#include +@[end for]@ +@[for idx, topic in enumerate(recv_topics)]@ +#include +@[end for]@ + +// Subscribers for messages to send +struct SendTopicsSubs { +@[ for idx, topic in enumerate(send_topics)]@ + uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; + uxrObjectId @(topic)_data_writer; +@[ end for]@ + + uxrSession* session; + + uint32_t num_payload_sent{}; + + bool init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id); + void update(uxrStreamId stream_id); +}; + +bool SendTopicsSubs::init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id) +{ + session = session_; + +@[ for idx, topic in enumerate(send_topics)]@ +@{ +topic_pascal = topic.replace("_", " ").title().replace(" ", "") +}@ + { + + uxrObjectId topic_id = uxr_object_id(@(idx)+1, UXR_TOPIC_ID); + const char* topic_xml = "" + "" + "rt/fmu/out/@(topic_pascal)" + "px4_msgs::msg::dds_::@(topic_pascal)_" + "" + ""; + uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, + UXR_REPLACE); + + uxrObjectId publisher_id = uxr_object_id(@(idx)+1, UXR_PUBLISHER_ID); + const char* publisher_xml = ""; + uint16_t publisher_req = uxr_buffer_create_publisher_xml(session, stream_id, publisher_id, participant_id, + publisher_xml, UXR_REPLACE); + + uxrObjectId datawriter_id = uxr_object_id(@(idx)+1, UXR_DATAWRITER_ID); + @(topic)_data_writer = datawriter_id; + const char* datawriter_xml = "" + "" + "" + "NO_KEY" + "rt/fmu/out/@(topic_pascal)" + "px4_msgs::msg::dds_::@(topic_pascal)_" + "" + "" + ""; + uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, stream_id, datawriter_id, publisher_id, + datawriter_xml, UXR_REPLACE); + + // Send create entities message and wait its status + uint8_t status[3]; + uint16_t requests[3] = { + topic_req, publisher_req, datawriter_req + }; + if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", "@(topic_pascal)", status[0], status[1], status[2]); + return false; + } + } + +@[ end for]@ + + return true; +} + +void SendTopicsSubs::update(uxrStreamId stream_id) +{ +@[ for idx, topic in enumerate(send_topics)]@ + { + @(send_base_types[idx])_s data; + + if (@(topic)_sub.update(&data)) { + ucdrBuffer ub{}; + uint32_t topic_size = ucdr_topic_size_@(send_base_types[idx])(); + uxr_prepare_output_stream(session, stream_id, @(topic)_data_writer, &ub, topic_size); + ucdr_serialize_@(send_base_types[idx])(data, ub); + // TODO: fill up the MTU and then flush, which reduces the packet overhead + uxr_flash_output_streams(session); + num_payload_sent += topic_size; + } + } +@[ end for]@ + +} + +static void on_topic_update(uxrSession* session, uxrObjectId object_id, + uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t + length, void* args); + +// Publishers for received messages +struct RcvTopicsPubs { +@[ for idx, topic in enumerate(recv_topics)]@ + uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; +@[ end for]@ + + uxrSession* session; + + uint32_t num_payload_received{}; + + bool init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id); +}; + +bool RcvTopicsPubs::init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id) +{ + session = session_; + uxr_set_topic_callback(session, on_topic_update, this); + + +@[ for idx, topic in enumerate(recv_topics)]@ +@{ +topic_pascal = topic.replace("_", " ").title().replace(" ", "") +}@ + { + + uxrObjectId subscriber_id = uxr_object_id(@(idx)+1, UXR_SUBSCRIBER_ID); + const char* subscriber_xml = ""; + uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(session, stream_id, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE); + + uxrObjectId topic_id = uxr_object_id(1000+@(idx), UXR_TOPIC_ID); + const char* topic_xml = "" + "" + "rt/fmu/in/@(topic_pascal)" + "px4_msgs::msg::dds_::@(topic_pascal)_" + "" + ""; + uint16_t topic_req = uxr_buffer_create_topic_xml(session, stream_id, topic_id, participant_id, topic_xml, UXR_REPLACE); + + uxrObjectId datareader_id = uxr_object_id(@(idx)+1, UXR_DATAREADER_ID); + const char* datareader_xml = "" + "" + "" + "NO_KEY" + "rt/fmu/in/@(topic_pascal)" + "px4_msgs::msg::dds_::@(topic_pascal)_" + "" + "" + ""; + uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, stream_id, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE); + + uint8_t status[3]; + uint16_t requests[3] = {topic_req, subscriber_req, datareader_req }; + if(!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) + { + PX4_ERR("create entities failed: %s %i %i %i", "@(topic)", status[0], status[1], status[2]); + return false; + } + + uxrDeliveryControl delivery_control = {0}; + delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED; + uxr_buffer_request_data(session, stream_id, datareader_id, input_stream, &delivery_control); + + } + +@[ end for]@ + + return true; +} + +void on_topic_update(uxrSession* session, uxrObjectId object_id, + uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t + length, void* args) +{ + RcvTopicsPubs* pubs = (RcvTopicsPubs*)args; + pubs->num_payload_received += length; + + switch (object_id.id) { +@[ for idx, topic in enumerate(recv_topics)]@ + case @(idx)+1: { + @(receive_base_types[idx])_s topic; + if (ucdr_deserialize_@(receive_base_types[idx])(*ub, topic)) { + pubs->@(topic)_pub.publish(topic); + } + } + break; +@[ end for]@ + + default: + PX4_ERR("unknown object id: %i", object_id.id); + break; + } +} diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em index 6953f5e9e5cb..1ab5e56e0c52 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em @@ -6,16 +6,15 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - msgs (List) list of all RTPS messages +@# - topics (List) list of all topic names +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ import os import genmsg.msgs -from px_generate_uorb_topic_helper import * # this is in Tools/ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ topic_names = [s.short_name for s in spec] @@ -27,7 +26,7 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] = /**************************************************************************** * * Copyright (c) 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -57,8 +56,8 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] = * ****************************************************************************/ -#include "microRTPS_transport.h" -#include "microRTPS_client.h" +#include +#include #include #include @@ -69,191 +68,239 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] = #include #include -#include +#include #include @[for topic in list(set(topic_names))]@ #include #include @[end for]@ -void* send(void *data); - -uint8_t last_remote_msg_seq = 0; -uint8_t last_msg_seq = 0; +using namespace time_literals; @[if recv_topics]@ // Publishers for received messages -struct RcvTopicsPubs -{ +struct RcvTopicsPubs { @[ for idx, topic in enumerate(recv_topics)]@ - uORB::Publication<@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; + uORB::Publication <@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))}; @[ end for]@ }; @[end if]@ @[if send_topics]@ // Subscribers for messages to send -struct SendTopicsSubs -{ +struct SendTopicsSubs { @[ for idx, topic in enumerate(send_topics)]@ - uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; + uORB::Subscription @(topic)_sub{ORB_ID(@(topic))}; @[ end for]@ }; -@[end if]@ +struct SendThreadArgs { + const uint32_t &datarate; + uint64_t &total_sent; + uint64_t &sent_last_sec; + uint64_t &sent; + int &sent_loop; + SendThreadArgs(const uint32_t &datarate_, uint64_t &total_sent_, + uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_) + : datarate(datarate_), + total_sent(total_sent_), + sent_last_sec(sent_last_sec_), + sent(sent_), + sent_loop(sent_loop_) {} +}; -@[if send_topics]@ -void* send(void* /*unused*/) +void *send(void *args) { - char data_buffer[BUFFER_SIZE] = {}; - uint64_t sent = 0, total_sent = 0; - int loop = 0, read = 0; - uint32_t length = 0; - size_t header_length = 0; - SendTopicsSubs *subs = new SendTopicsSubs(); - - // ucdrBuffer to serialize using the user defined buffer - ucdrBuffer writer; - header_length = transport_node->get_header_length(); - ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); - - struct timespec begin; - px4_clock_gettime(CLOCK_REALTIME, &begin); - - while (!_should_exit_task) - { -@[for idx, topic in enumerate(send_topics)]@ - { - @(send_base_types[idx])_s @(topic)_data; - if (subs->@(topic)_sub.update(&@(topic)_data)) { -@[if topic == 'Timesync' or topic == 'timesync']@ - if(@(topic)_data.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) { - last_remote_msg_seq = @(topic)_data.seq; - - @(topic)_data.timestamp = hrt_absolute_time(); - @(topic)_data.sys_id = 1; - @(topic)_data.seq = last_msg_seq; - @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL; - @(topic)_data.ts1 = @(topic)_data.ts1; - - last_msg_seq++; -@[end if]@ - // copy raw data into local buffer. Payload is shifted by header length to make room for header - serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); - if (0 < (read = transport_node->write(static_cast(@(rtps_message_id(ids, topic))), data_buffer, length))) - { - total_sent += read; - ++sent; - } -@[if topic == 'Timesync' or topic == 'timesync']@ - } -@[end if]@ - } - } -@[end for]@ - px4_usleep(_options.sleep_ms * 1000); - ++loop; - } + char data_buffer[BUFFER_SIZE]{}; + int read{0}; + uint32_t length{0}; + size_t header_length{0}; + uint8_t last_msg_seq{0}; + uint8_t last_remote_msg_seq{0}; + + struct SendThreadArgs *data = reinterpret_cast(args); + SendTopicsSubs *subs = new SendTopicsSubs(); + + float bandwidth_mult{0}; + float tx_interval{1.f}; + uint64_t tx_last_sec_read{0}; + hrt_abstime last_stats_update{0}; + + // ucdrBuffer to serialize using the user defined buffer + ucdrBuffer writer; + header_length = transport_node->get_header_length(); + ucdr_init_buffer(&writer, reinterpret_cast(&data_buffer[header_length]), BUFFER_SIZE - header_length); + + while (!_should_exit_task) { +@[ for idx, topic in enumerate(send_topics)]@ + { + @(send_base_types[idx])_s @(topic)_data; + + if (subs->@(topic)_sub.update(&@(topic)_data)) + { +@[ if topic == 'Timesync' or topic == 'timesync']@ + if (@(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) { + last_remote_msg_seq = @(topic)_data.seq; + + @(topic)_data.timestamp = hrt_absolute_time(); + @(topic)_data.seq = last_msg_seq; + @(topic)_data.tc1 = hrt_absolute_time() * 1000ULL; + @(topic)_data.ts1 = @(topic)_data.ts1; + + last_msg_seq++; +@[ end if]@ + // copy raw data into local buffer. Payload is shifted by header length to make room for header + serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length); + + if (0 < (read = transport_node->write(static_cast(@(msgs[0].index(topic) + 1)), data_buffer, length))) { + data->total_sent += read; + tx_last_sec_read += read; + ++data->sent; + } + +@[ if topic == 'Timesync' or topic == 'timesync']@ + } + +@[ end if]@ + } + } +@[ end for]@ - struct timespec end; - px4_clock_gettime(CLOCK_REALTIME, &end); - double elapsed_secs = end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9; - PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s", - sent, loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs)); + if (hrt_absolute_time() - last_stats_update >= 1_s) { + data->sent_last_sec = tx_last_sec_read; + if (data->datarate > 0) { + bandwidth_mult = static_cast(data->datarate) / static_cast(tx_last_sec_read); + // Apply a low-pass filter to determine the new TX interval + tx_interval += 0.5f * (tx_interval / bandwidth_mult - tx_interval); + // Clamp the interval between 1 and 1000 ms + tx_interval = math::constrain(tx_interval, MIN_TX_INTERVAL_US, MAX_TX_INTERVAL_US); + } + tx_last_sec_read = 0; + last_stats_update = hrt_absolute_time(); + } - delete subs; + px4_usleep(tx_interval); - return nullptr; + ++data->sent_loop; + } + + delete(data); + delete(subs); + + return nullptr; } -static int launch_send_thread(pthread_t &sender_thread) +static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args) { - pthread_attr_t sender_thread_attr; - pthread_attr_init(&sender_thread_attr); - pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250)); - struct sched_param param; - (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_DEFAULT; - (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); - pthread_create(&sender_thread, &sender_thread_attr, send, nullptr); - if (pthread_setname_np(sender_thread, "micrortps_client_send")) - { - PX4_ERR("Could not set pthread name (%d)", errno); - } - pthread_attr_destroy(&sender_thread_attr); - - return 0; + pthread_attr_t sender_thread_attr; + pthread_attr_init(&sender_thread_attr); + pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250)); + struct sched_param param; + (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_DEFAULT; + (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); + int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args); + if (rc != 0) { + errno = rc; + PX4_ERR("Could not create send thread (%d)", errno); + return -1; + } + rc = pthread_setname_np(sender_thread, "urtpsclient_snd"); + if (pthread_setname_np(sender_thread, "urtpsclient_snd")) { + errno = rc; + PX4_ERR("Could not set pthread name for the send thread (%d)", errno); + } + pthread_attr_destroy(&sender_thread_attr); + + return 0; } @[end if]@ -void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop) +void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd, + uint64_t &total_sent, uint64_t &sent_last_sec, + uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop) { + px4_clock_gettime(CLOCK_REALTIME, &begin); + _should_exit_task = false; + @[if recv_topics]@ - char data_buffer[BUFFER_SIZE] = {}; - int read = 0; - uint8_t topic_ID = 255; - RcvTopicsPubs *pubs = new RcvTopicsPubs(); - - // Set the main task name to 'micrortps_client_rcv' in case there is - // data to receive - px4_prctl(PR_SET_NAME, "micrortps_client_rcv", px4_getpid()); - - // ucdrBuffer to deserialize using the user defined buffer - ucdrBuffer reader; - ucdr_init_buffer(&reader, reinterpret_cast(data_buffer), BUFFER_SIZE); + char data_buffer[BUFFER_SIZE]{}; + int read{0}; + uint8_t topic_ID{255}; + + uint64_t rx_last_sec_read{0}; + hrt_abstime last_stats_update{0}; + + RcvTopicsPubs *pubs = new RcvTopicsPubs(); + + // Set the main task name to 'urtpsclient_rcv' in case there is + // data to receive + px4_prctl(PR_SET_NAME, "urtpsclient_rcv", px4_getpid()); + + // ucdrBuffer to deserialize using the user defined buffer + ucdrBuffer reader; + ucdr_init_buffer(&reader, reinterpret_cast(data_buffer), BUFFER_SIZE); @[end if]@ - px4_clock_gettime(CLOCK_REALTIME, &begin); - _should_exit_task = false; @[if send_topics]@ + // var struct to be updated on the thread + SendThreadArgs *sender_thread_args = new SendThreadArgs(datarate, total_sent, sent_last_sec, sent, sent_loop); - // create a thread for sending data to the simulator - pthread_t sender_thread; - launch_send_thread(sender_thread); + // create a thread for sending data + pthread_t sender_thread; + launch_send_thread(sender_thread, (*sender_thread_args)); @[end if]@ - while (!_should_exit_task) - { + while (!_should_exit_task) { @[if recv_topics]@ - while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) - { - total_read += read; - uint64_t read_time = hrt_absolute_time(); - switch (topic_ID) - { -@[for idx, topic in enumerate(recv_topics)]@ - case @(rtps_message_id(ids, topic)): - { - @(receive_base_types[idx])_s @(topic)_data; - deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); - if (@(topic)_data.timestamp > read_time) - { - // don't allow timestamps from the future - @(topic)_data.timestamp = read_time; - } - pubs->@(topic)_pub.publish(@(topic)_data); - ++received; - } - break; -@[end for]@ - default: - PX4_WARN("Unexpected topic ID '%hhu' to getMsg. Please make sure the client is capable of parsing the message associated to the topic ID '%hhu'", topic_ID, topic_ID); - break; - } - } -@[end if]@ + while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) { + total_rcvd += read; + rx_last_sec_read += read; - // loop forever if informed loop number is negative - if (_options.loops >= 0 && loop >= _options.loops) break; + uint64_t read_time = hrt_absolute_time(); - px4_usleep(_options.sleep_ms * 1000); - ++loop; - } -@[if recv_topics]@ - delete pubs; + switch (topic_ID) { +@[ for idx, topic in enumerate(recv_topics)]@ + case @(msgs[0].index(topic) + 1): { + @(receive_base_types[idx])_s @(topic)_data; + deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer); + + if (@(topic)_data.timestamp > read_time) { + // don't allow timestamps from the future + @(topic)_data.timestamp = read_time; + } + + pubs->@(topic)_pub.publish(@(topic)_data); + ++received; + } + break; +@[ end for]@ + default: + PX4_WARN("Unexpected topic ID '%hhu' to getMsg. Please make sure the client is capable of parsing the message associated to the topic ID '%hhu'", + topic_ID, topic_ID); + break; + } + } @[end if]@ + + if (hrt_absolute_time() - last_stats_update >= 1_s) { + rcvd_last_sec = rx_last_sec_read; + rx_last_sec_read = 0; + last_stats_update = hrt_absolute_time(); + } + + // loop forever if informed loop number is negative + if (_options.loops >= 0 && rcvd_loop >= _options.loops) { break; } + + px4_usleep(_options.sleep_us); + ++rcvd_loop; + } + @[if send_topics]@ - _should_exit_task = true; - pthread_join(sender_thread, nullptr); + _should_exit_task = true; + pthread_join(sender_thread, nullptr); +@[end if]@ +@[if recv_topics]@ + delete(pubs); @[end if]@ } diff --git a/msg/templates/uorb_microcdr/msg.cpp.em b/msg/templates/uorb_microcdr/msg.cpp.em index ea7251c47bea..458f455f498d 100644 --- a/msg/templates/uorb_microcdr/msg.cpp.em +++ b/msg/templates/uorb_microcdr/msg.cpp.em @@ -12,14 +12,11 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification @# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 diff --git a/msg/templates/uorb_microcdr/msg.h.em b/msg/templates/uorb_microcdr/msg.h.em index ce49dd1356e5..76b793ab706e 100644 --- a/msg/templates/uorb_microcdr/msg.h.em +++ b/msg/templates/uorb_microcdr/msg.h.em @@ -12,14 +12,10 @@ @# Context: @# - file_name_in (String) Source file @# - spec (msggen.MsgSpec) Parsed specification of the .msg file -@# - md5sum (String) MD5Sum of the .msg specification -@# - search_path (dict) search paths for genmsg -@# - topics (List of String) multi-topic names -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 diff --git a/msg/templates/uorb_microcdr/uORBTopics.cpp.em b/msg/templates/uorb_microcdr/uORBTopics.cpp.em index a4ee51ece962..88dc5255d8a6 100644 --- a/msg/templates/uorb_microcdr/uORBTopics.cpp.em +++ b/msg/templates/uorb_microcdr/uORBTopics.cpp.em @@ -5,15 +5,10 @@ @# @############################################### @# Start of Template -@# -@# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 diff --git a/msg/templates/uorb_microcdr/uORBTopics.hpp.em b/msg/templates/uorb_microcdr/uORBTopics.hpp.em index 5b1954d17af5..6eebd31544c9 100644 --- a/msg/templates/uorb_microcdr/uORBTopics.hpp.em +++ b/msg/templates/uorb_microcdr/uORBTopics.hpp.em @@ -5,15 +5,10 @@ @# @############################################### @# Start of Template -@# -@# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids @############################################### /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-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 diff --git a/msg/templates/urtps/Publisher.cpp.em b/msg/templates/urtps/Publisher.cpp.em index 8454b522217d..86687f06a1d8 100644 --- a/msg/templates/urtps/Publisher.cpp.em +++ b/msg/templates/urtps/Publisher.cpp.em @@ -6,26 +6,37 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - fastrtps_version (str) FastRTPS version installed on the system +@# - ros2_distro (str) ROS2 distro name +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ -from packaging import version import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +from packaging import version +import re topic = alias if alias else spec.short_name + try: ros2_distro = ros2_distro.decode("utf-8") except AttributeError: pass + +topic_name = topic + +# For ROS, use the topic pattern convention defined in +# http://wiki.ros.org/ROS/Patterns/Conventions +if ros2_distro: + topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() + topic_name = topic_name_split[0] + for w in topic_name_split[1:]: + topic_name += "_" + w + topic_name = topic_name.lower() }@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -59,105 +70,150 @@ except AttributeError: * @@file @(topic)_Publisher.cpp * This file contains the implementation of the publisher functions. * - * This file was adapted from the fastcdrgen tool. + * This file was adapted from the fastrtpsgen tool. */ +#include "@(topic)_Publisher.h" +#include #include #include #include #include +#include +@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ +#include -#include +using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; +@[end if]@ -#include "@(topic)_Publisher.h" @(topic)_Publisher::@(topic)_Publisher() - : mp_participant(nullptr), - mp_publisher(nullptr) + : mp_participant(nullptr), + mp_publisher(nullptr) { } @(topic)_Publisher::~@(topic)_Publisher() { - Domain::removeParticipant(mp_participant); + Domain::removeParticipant(mp_participant); } -bool @(topic)_Publisher::init(const std::string& ns) +bool @(topic)_Publisher::init(const std::string &ns, std::string topic_name) { - // Create RTPSParticipant - ParticipantAttributes PParam; + // Create RTPSParticipant + ParticipantAttributes PParam; @[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; + PParam.rtps.builtin.domainId = 0; @[else]@ - PParam.domainId = 0; + PParam.domainId = 0; @[end if]@ @[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; + PParam.rtps.builtin.leaseDuration = c_TimeInfinite; @[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; + PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; @[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_publisher"); - PParam.rtps.setName(nodeName.c_str()); - mp_participant = Domain::createParticipant(PParam); - if(mp_participant == nullptr) - return false; - - // Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Publisher - PublisherAttributes Wparam; - Wparam.topic.topicKind = NO_KEY; - Wparam.topic.topicDataType = @(topic)DataType.getName(); +@[if ros2_distro]@ + // ROS2 default memory management policy + PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; +@[end if]@ + std::string nodeName = ns; + nodeName.append("@(topic)_publisher"); + PParam.rtps.setName(nodeName.c_str()); + +@[if ros2_distro]@ + // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only + // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and + // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used + const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); + const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); + const char* ros_distro = std::getenv("ROS_DISTRO"); + if (localhost_only && strcmp(localhost_only, "1") == 0 + && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) + || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) + || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { + // Create a custom network UDPv4 transport descriptor + // to whitelist the localhost + auto localhostUdpTransport = std::make_shared(); + localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); + + // Disable the built-in Transport Layer + PParam.rtps.useBuiltinTransports = false; + + // Add the descriptor as a custom user transport + PParam.rtps.userTransports.push_back(localhostUdpTransport); + +@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ + // Add shared memory transport when available + auto shmTransport = std::make_shared(); + PParam.rtps.userTransports.push_back(shmTransport); +@[ end if]@ + } +@[end if]@ + + mp_participant = Domain::createParticipant(PParam); + + if (mp_participant == nullptr) { + return false; + } + + // Register the type + Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); + + // Create Publisher + PublisherAttributes Wparam; + Wparam.topic.topicKind = NO_KEY; + Wparam.topic.topicDataType = @(topic)DataType.getName(); @[if ros2_distro]@ @[ if ros2_distro == "ardent"]@ - Wparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; - topicName.append("@(topic)_PubSubTopic"); - Wparam.topic.topicName = topicName; + Wparam.qos.m_partition.push_back("rt"); + std::string topicName = ns; @[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); - topicName.append("@(topic)_PubSubTopic"); - Wparam.topic.topicName = topicName; + std::string topicName = "rt/"; + topicName.append(ns); @[ end if]@ + // ROS2 default publish mode QoS policy + Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE; @[else]@ - std::string topicName = ns; - topicName.append("@(topic)PubSubTopic"); - Wparam.topic.topicName = topicName; + std::string topicName = ns; @[end if]@ - mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); - if(mp_publisher == nullptr) - return false; - return true; + topic_name.empty() ? topicName.append("fmu/@(topic_name)/out") : topicName.append(topic_name); + Wparam.topic.topicName = topicName; + mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast(&m_listener)); + + if (mp_publisher == nullptr) { + return false; + } + + return true; } -void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub, MatchingInfo& info) +void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher *pub, MatchingInfo &info) { - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - for (size_t i = 0; i < 6; i++) { - if (pub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl; - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl; - } - } + // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain + // are the same for all its subcomponents (publishers, subscribers) + bool is_different_endpoint = false; + + for (size_t i = 0; i < 6; i++) { + if (pub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { + is_different_endpoint = true; + break; + } + } + + // If the matching happens for the same entity, do not make a match + if (is_different_endpoint) { + if (info.status == MATCHED_MATCHING) { + n_matched++; + std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl; + + } else { + n_matched--; + std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl; + } + } } -void @(topic)_Publisher::publish(@(topic)_msg_t* st) +void @(topic)_Publisher::publish(@(topic)_msg_t *st) { - mp_publisher->write(st); + mp_publisher->write(st); } diff --git a/msg/templates/urtps/Publisher.h.em b/msg/templates/urtps/Publisher.h.em index 76236b832b23..6851497d2a5f 100644 --- a/msg/templates/urtps/Publisher.h.em +++ b/msg/templates/urtps/Publisher.h.em @@ -6,15 +6,13 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - ros2_distro (str) ROS2 distro name +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ -from packaging import version import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +from packaging import version +import re topic = alias if alias else spec.short_name try: @@ -25,7 +23,7 @@ except AttributeError: /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -59,7 +57,7 @@ except AttributeError: * @@file @(topic)_Publisher.h * This header file contains the declaration of the publisher functions. * - * This file was adapted from the fastcdrgen tool. + * This file was adapted from the fastrtpsgen tool. */ @@ -99,24 +97,24 @@ using @(topic)_msg_datatype = @(topic)PubSubType; class @(topic)_Publisher { public: - @(topic)_Publisher(); - virtual ~@(topic)_Publisher(); - bool init(const std::string& ns); - void run(); - void publish(@(topic)_msg_t* st); + @(topic)_Publisher(); + virtual ~@(topic)_Publisher(); + bool init(const std::string &ns, std::string topic_name = ""); + void run(); + void publish(@(topic)_msg_t *st); private: - Participant *mp_participant; - Publisher *mp_publisher; + Participant *mp_participant; + Publisher *mp_publisher; - class PubListener : public PublisherListener - { - public: - PubListener() : n_matched(0){}; - ~PubListener(){}; - void onPublicationMatched(Publisher* pub, MatchingInfo& info); - int n_matched; - } m_listener; - @(topic)_msg_datatype @(topic)DataType; + class PubListener : public PublisherListener + { + public: + PubListener() : n_matched(0) {}; + ~PubListener() {}; + void onPublicationMatched(Publisher *pub, MatchingInfo &info); + int n_matched; + } m_listener; + @(topic)_msg_datatype @(topic)DataType; }; #endif // _@(topic)__PUBLISHER_H_ diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index e9e1b271053a..be1d8d20459f 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -6,25 +6,20 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - ids (List) list of all RTPS msg ids +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ -import os - import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +import os from px_generate_uorb_topic_files import MsgScope # this is in Tools/ send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] -package = package[0] }@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -56,110 +51,137 @@ package = package[0] #include "RtpsTopics.h" -bool RtpsTopics::init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue, const std::string& ns) +bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, + std::queue *t_send_queue, const std::string &ns) { @[if recv_topics]@ - // Initialise subscribers - std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl; + // Initialise subscribers + std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl; @[for topic in recv_topics]@ - if (_@(topic)_sub.init(@(rtps_message_id(ids, topic)), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) { - std::cout << "- @(topic) subscriber started" << std::endl; - } else { - std::cerr << "Failed starting @(topic) subscriber" << std::endl; - return false; - } + + if (_@(topic)_sub.init(@(msgs[0].index(topic) + 1), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) { + std::cout << "- @(topic) subscriber started" << std::endl; + + } else { + std::cerr << "Failed starting @(topic) subscriber" << std::endl; + return false; + } + @[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl; + std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl; @[end if]@ @[if send_topics]@ - // Initialise publishers - std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl; + + // Initialise publishers + std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl; @[for topic in send_topics]@ - if (_@(topic)_pub.init(ns)) { - std::cout << "- @(topic) publisher started" << std::endl; + @[ if topic == 'Timesync' or topic == 'timesync']@ - _timesync->start(&_@(topic)_pub); + if (_@(topic)_pub.init(ns)) { + if (_@(topic)_fmu_in_pub.init(ns, std::string("fmu/timesync/in"))) { + _timesync->start(&_@(topic)_fmu_in_pub); + std::cout << "- @(topic) publishers started" << std::endl; + } +@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@ + if (_@(topic)_pub.init(ns, std::string("timesync_status"))) { + _timesync->init_status_pub(&_@(topic)_pub); + std::cout << "- @(topic) publisher started" << std::endl; +@[ else]@ + if (_@(topic)_pub.init(ns)) { + std::cout << "- @(topic) publisher started" << std::endl; @[ end if]@ - } else { - std::cerr << "ERROR starting @(topic) publisher" << std::endl; - return false; - } + + } else { + std::cerr << "ERROR starting @(topic) publisher" << std::endl; + return false; + } + @[end for]@ - std::cout << "\033[0;36m-----------------------\033[0m" << std::endl; + std::cout << "\033[0;36m-----------------------\033[0m" << std::endl; @[end if]@ - return true; + return true; } @[if send_topics]@ -void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len) +template +void RtpsTopics::sync_timestamp_of_incoming_data(T &msg) { + uint64_t timestamp = getMsgTimestamp(&msg); + uint64_t timestamp_sample = getMsgTimestampSample(&msg); + _timesync->subtractOffset(timestamp); + setMsgTimestamp(&msg, timestamp); + _timesync->subtractOffset(timestamp_sample); + setMsgTimestampSample(&msg, timestamp_sample); +} + +void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len) { - switch (topic_ID) - { + switch (topic_ID) { @[for topic in send_topics]@ - case @(rtps_message_id(ids, topic)): // @(topic) - { - @(topic)_msg_t st; - eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); - eprosima::fastcdr::Cdr cdr_des(cdrbuffer); - st.deserialize(cdr_des); -@[ if topic == 'Timesync' or topic == 'timesync']@ - _timesync->processTimesyncMsg(&st); - if (getMsgSysID(&st) == 1) { -@[ end if]@ - // apply timestamp offset - uint64_t timestamp = getMsgTimestamp(&st); - _timesync->subtractOffset(timestamp); - setMsgTimestamp(&st, timestamp); - _@(topic)_pub.publish(&st); + case @(msgs[0].index(topic) + 1): { // @(topic) publisher + @(topic)_msg_t st; + eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len); + eprosima::fastcdr::Cdr cdr_des(cdrbuffer); + st.deserialize(cdr_des); @[ if topic == 'Timesync' or topic == 'timesync']@ - } + _timesync->processTimesyncMsg(&st, &_@(topic)_pub); @[ end if]@ - } - break; + + // apply timestamp offset + sync_timestamp_of_incoming_data(st); + + _@(topic)_pub.publish(&st); + } + break; @[end for]@ - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to publish Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", topic_ID, topic_ID); - break; - } + + default: + printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to publish. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", + topic_ID, topic_ID); + break; + } } @[end if]@ @[if recv_topics]@ +template +void RtpsTopics::sync_timestamp_of_outgoing_data(T &msg) { + uint64_t timestamp = getMsgTimestamp(&msg); + uint64_t timestamp_sample = getMsgTimestampSample(&msg); + _timesync->addOffset(timestamp); + setMsgTimestamp(&msg, timestamp); + _timesync->addOffset(timestamp_sample); + setMsgTimestampSample(&msg, timestamp_sample); +} bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) { - bool ret = false; - switch (topic_ID) - { + bool ret = false; + + switch (topic_ID) { @[for topic in recv_topics]@ - case @(rtps_message_id(ids, topic)): // @(topic) - if (_@(topic)_sub.hasMsg()) - { - @(topic)_msg_t msg = _@(topic)_sub.getMsg(); -@[ if topic == 'Timesync' or topic == 'timesync']@ - if (getMsgSysID(&msg) == 0) { -@[ end if]@ - // apply timestamps offset - uint64_t timestamp = getMsgTimestamp(&msg); - uint64_t timestamp_sample = getMsgTimestampSample(&msg); - _timesync->addOffset(timestamp); - setMsgTimestamp(&msg, timestamp); - _timesync->addOffset(timestamp_sample); - setMsgTimestampSample(&msg, timestamp_sample); - msg.serialize(scdr); - ret = true; -@[ if topic == 'Timesync' or topic == 'timesync']@ - } -@[ end if]@ - _@(topic)_sub.unlockMsg(); - } - break; + + case @(msgs[0].index(topic) + 1): // @(topic) subscriber + if (_@(topic)_sub.hasMsg()) { + @(topic)_msg_t msg = _@(topic)_sub.getMsg(); + + // apply timestamp offset + sync_timestamp_of_outgoing_data(msg); + + msg.serialize(scdr); + ret = true; + + _@(topic)_sub.unlockMsg(); + } + + break; @[end for]@ - default: - printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", topic_ID, topic_ID); - break; - } - return ret; + default: + printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n", + topic_ID, topic_ID); + break; + } + + return ret; } @[end if]@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index 4f3f9d4e15fb..b82c7b20c8c5 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/msg/templates/urtps/RtpsTopics.h.em @@ -6,16 +6,15 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - ids (List) list of all RTPS msg ids +@# - fastrtps_version (List[str]) FastRTPS version installed on the system +@# - package (List[str]) messages package name. Defaulted to 'px4' +@# - ros2_distro (List[str]) ROS2 distro name +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ +import genmsg.msgs import os from packaging import version - -import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] @@ -30,7 +29,7 @@ except AttributeError: /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -91,117 +90,131 @@ using @(topic)_msg_t = @(topic); @[ end if]@ @[end for]@ -class RtpsTopics { +class RtpsTopics +{ public: - bool init(std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue, const std::string& ns); - void set_timesync(const std::shared_ptr& timesync) { _timesync = timesync; }; + bool init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, std::queue *t_send_queue, + const std::string &ns); + void set_timesync(const std::shared_ptr ×ync) { _timesync = timesync; }; @[if send_topics]@ - void publish(uint8_t topic_ID, char data_buffer[], size_t len); + template + void sync_timestamp_of_incoming_data(T &msg); + void publish(const uint8_t topic_ID, char data_buffer[], size_t len); @[end if]@ @[if recv_topics]@ - bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr); + template + void sync_timestamp_of_outgoing_data(T &msg); + bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr); @[end if]@ private: @[if send_topics]@ - /** Publishers **/ + /** Publishers **/ @[for topic in send_topics]@ - @(topic)_Publisher _@(topic)_pub; +@[ if topic == 'Timesync' or topic == 'timesync']@ + @(topic)_Publisher _@(topic)_pub; + @(topic)_Publisher _@(topic)_fmu_in_pub; +@[ else]@ + @(topic)_Publisher _@(topic)_pub; +@[ end if]@ @[end for]@ @[end if]@ @[if recv_topics]@ - /** Subscribers **/ + /** Subscribers **/ @[for topic in recv_topics]@ - @(topic)_Subscriber _@(topic)_sub; + @(topic)_Subscriber _@(topic)_sub; @[end for]@ @[end if]@ - // SFINAE - template struct hasTimestampSample{ - private: - static void detect(...); - template static decltype(std::declval().timestamp_sample()) detect(const U&); - public: - static constexpr bool value = std::is_same()))>::value; - }; - - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T*) { return 0; } - - /** Msg metada Getters **/ + // SFINAE + template struct hasTimestampSample{ + private: + template().timestamp_sample(int64_t()))> + static std::true_type detect(int); + template + static std::false_type detect(...); + public: + static constexpr bool value = decltype(detect(0))::value; + }; + + template + inline typename std::enable_if < !hasTimestampSample::value, uint64_t >::type + getMsgTimestampSample_impl(const T *) { return 0; } + + /** Msg metada Getters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); } + template + inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp_(); } - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample_(); } + template + inline typename std::enable_if::value, uint64_t>::type + getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample_(); } - template - inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); } + template + inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id_(); } - template - inline uint8_t getMsgSeq(const T* msg) { return msg->seq_(); } + template + inline uint8_t getMsgSeq(const T *msg) { return msg->seq_(); } @[elif ros2_distro]@ - template - inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); } + template + inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp(); } - template - inline typename std::enable_if::value, uint64_t>::type - getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample(); } + template + inline typename std::enable_if::value, uint64_t>::type + getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample(); } - template - inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id(); } + template + inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id(); } - template - inline uint8_t getMsgSeq(const T* msg) { return msg->seq(); } + template + inline uint8_t getMsgSeq(const T *msg) { return msg->seq(); } @[end if]@ - template - inline uint64_t getMsgTimestampSample(const T* msg) { return getMsgTimestampSample_impl(msg); } + template + inline uint64_t getMsgTimestampSample(const T *msg) { return getMsgTimestampSample_impl(msg); } - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T*, const uint64_t&) {} + template + inline typename std::enable_if ::value, void>::type + setMsgTimestampSample_impl(T *, const uint64_t &) {} - /** Msg metadata Setters **/ + /** Msg metadata Setters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - template - inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; } + template + inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample_() = timestamp_sample; } + template + inline typename std::enable_if::value, void>::type + setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample_() = timestamp_sample; } - template - inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; } + template + inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id_() = sys_id; } - template - inline void setMsgSeq(T* msg, const uint8_t& seq) { msg->seq_() = seq; } + template + inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq_() = seq; } @[elif ros2_distro]@ - template - inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; } + template + inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } - template - inline typename std::enable_if::value, void>::type - setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample() = timestamp_sample; } + template + inline typename std::enable_if::value, void>::type + setMsgTimestampSample_impl(T *msg, const uint64_t ×tamp_sample) { msg->timestamp_sample() = timestamp_sample; } - template - inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; } + template + inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id() = sys_id; } - template - inline void setMsgSeq(T* msg, const uint8_t& seq) { msg->seq() = seq; } + template + inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq() = seq; } @[end if]@ - template - inline void setMsgTimestampSample(T* msg, const uint64_t& timestamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); } + template + inline void setMsgTimestampSample(T *msg, const uint64_t ×tamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); } - /** - * @@brief Timesync object ptr. - * This object is used to compuyte and apply the time offsets to the - * messages timestamps. - */ - std::shared_ptr _timesync; + /** + * @@brief Timesync object ptr. + * This object is used to compuyte and apply the time offsets to the + * messages timestamps. + */ + std::shared_ptr _timesync; }; diff --git a/msg/templates/urtps/Subscriber.cpp.em b/msg/templates/urtps/Subscriber.cpp.em index 4cb048d74e0e..ab0c5f2d82c8 100644 --- a/msg/templates/urtps/Subscriber.cpp.em +++ b/msg/templates/urtps/Subscriber.cpp.em @@ -6,26 +6,36 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - fastrtps_version (str) FastRTPS version installed on the system +@# - ros2_distro (str) ROS2 distro name +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ -from packaging import version import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +from packaging import version +import re topic = alias if alias else spec.short_name try: ros2_distro = ros2_distro.decode("utf-8") except AttributeError: pass + +topic_name = topic + +# For ROS, use the topic pattern convention defined in +# http://wiki.ros.org/ROS/Patterns/Conventions +if ros2_distro: + topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split() + topic_name = topic_name_split[0] + for w in topic_name_split[1:]: + topic_name += "_" + w + topic_name = topic_name.lower() }@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -59,169 +69,216 @@ except AttributeError: * @@file @(topic)_Subscriber.cpp * This file contains the implementation of the subscriber functions. * - * This file was adapted from the fastcdrgen tool. + * This file was adapted from the fastrtpsgen tool. */ +#include "@(topic)_Subscriber.h" + +#include #include #include #include #include +#include +@[if version.parse(fastrtps_version) >= version.parse('2.0')]@ +#include -#include +using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor; +@[end if]@ -#include "@(topic)_Subscriber.h" @(topic)_Subscriber::@(topic)_Subscriber() - : mp_participant(nullptr), - mp_subscriber(nullptr) + : mp_participant(nullptr), + mp_subscriber(nullptr) { } @(topic)_Subscriber::~@(topic)_Subscriber() { - Domain::removeParticipant(mp_participant); + Domain::removeParticipant(mp_participant); } -bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue, const std::string& ns) +bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, + std::mutex *t_send_queue_mutex, std::queue *t_send_queue, const std::string &ns, + std::string topic_name) { - m_listener.topic_ID = topic_ID; - m_listener.t_send_queue_cv = t_send_queue_cv; - m_listener.t_send_queue_mutex = t_send_queue_mutex; - m_listener.t_send_queue = t_send_queue; + m_listener.topic_ID = topic_ID; + m_listener.t_send_queue_cv = t_send_queue_cv; + m_listener.t_send_queue_mutex = t_send_queue_mutex; + m_listener.t_send_queue = t_send_queue; - // Create RTPSParticipant - ParticipantAttributes PParam; + // Create RTPSParticipant + ParticipantAttributes PParam; @[if version.parse(fastrtps_version) < version.parse('2.0')]@ - PParam.rtps.builtin.domainId = 0; + PParam.rtps.builtin.domainId = 0; @[else]@ - PParam.domainId = 0; + PParam.domainId = 0; @[end if]@ @[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@ - PParam.rtps.builtin.leaseDuration = c_TimeInfinite; + PParam.rtps.builtin.leaseDuration = c_TimeInfinite; @[else]@ - PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; + PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite; @[end if]@ - std::string nodeName = ns; - nodeName.append("@(topic)_subscriber"); - PParam.rtps.setName(nodeName.c_str()); - mp_participant = Domain::createParticipant(PParam); - if(mp_participant == nullptr) - return false; - - //Register the type - Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); - - // Create Subscriber - SubscriberAttributes Rparam; - Rparam.topic.topicKind = NO_KEY; - Rparam.topic.topicDataType = @(topic)DataType.getName(); +@[if ros2_distro]@ + // ROS2 default memory management policy + PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE; +@[end if]@ + std::string nodeName = ns; + nodeName.append("@(topic)_subscriber"); + PParam.rtps.setName(nodeName.c_str()); + +@[if ros2_distro]@ + // Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only + // the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and + // RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used + const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); + const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION"); + const char* ros_distro = std::getenv("ROS_DISTRO"); + if (localhost_only && strcmp(localhost_only, "1") == 0 + && ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0) + || (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0))) + || (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) { + // Create a custom network UDPv4 transport descriptor + // to whitelist the localhost + auto localhostUdpTransport = std::make_shared(); + localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1"); + + // Disable the built-in Transport Layer + PParam.rtps.useBuiltinTransports = false; + + // Add the descriptor as a custom user transport + PParam.rtps.userTransports.push_back(localhostUdpTransport); + +@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@ + // Add shared memory transport when available + auto shmTransport = std::make_shared(); + PParam.rtps.userTransports.push_back(shmTransport); +@[ end if]@ + } +@[end if]@ + + mp_participant = Domain::createParticipant(PParam); + + if (mp_participant == nullptr) { + return false; + } + + // Register the type + Domain::registerType(mp_participant, static_cast(&@(topic)DataType)); + + // Create Subscriber + SubscriberAttributes Rparam; + Rparam.topic.topicKind = NO_KEY; + Rparam.topic.topicDataType = @(topic)DataType.getName(); @[if ros2_distro]@ @[ if ros2_distro == "ardent"]@ - Rparam.qos.m_partition.push_back("rt"); - std::string topicName = ns; - topicName.append("@(topic)_PubSubTopic"); - Rparam.topic.topicName = topicName; + Rparam.qos.m_partition.push_back("rt"); + std::string topicName = ns; @[ else]@ - std::string topicName = "rt/"; - topicName.append(ns); - topicName.append("@(topic)_PubSubTopic"); - Rparam.topic.topicName = topicName; + std::string topicName = "rt/"; + topicName.append(ns); @[ end if]@ @[else]@ - std::string topicName = ns; - topicName.append("@(topic)PubSubTopic"); - Rparam.topic.topicName = topicName; + std::string topicName = ns; @[end if]@ - mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast(&m_listener)); - if(mp_subscriber == nullptr) - return false; - return true; + topic_name.empty() ? topicName.append("fmu/@(topic_name)/in") : topicName.append(topic_name); + Rparam.topic.topicName = topicName; + mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast(&m_listener)); + + if (mp_subscriber == nullptr) { + return false; + } + + return true; } -void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub, MatchingInfo& info) +void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber *sub, MatchingInfo &info) { @# Since the time sync runs on the bridge itself, it is required that there is a @# match between two topics of the same entity -@[if topic != 'Timesync' and topic != 'timesync']@ - // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain - // are the same for all its subcomponents (publishers, subscribers) - bool is_different_endpoint = false; - for (size_t i = 0; i < 6; i++) { - if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { - is_different_endpoint = true; - break; - } - } - - // If the matching happens for the same entity, do not make a match - if (is_different_endpoint) { - if (info.status == MATCHED_MATCHING) { - n_matched++; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl; - } else { - n_matched--; - std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl; - } - } +@[if topic != 'Timesync' and topic != 'timesync' and topic != 'TimesyncStatus' and topic != 'timesync_status']@ + // The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain + // are the same for all its subcomponents (publishers, subscribers) + bool is_different_endpoint = false; + + for (size_t i = 0; i < 6; i++) { + if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) { + is_different_endpoint = true; + break; + } + } + + // If the matching happens for the same entity, do not make a match + if (is_different_endpoint) { + if (info.status == MATCHED_MATCHING) { + n_matched++; + std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl; + + } else { + n_matched--; + std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl; + } + } + @[else]@ - (void)sub; + (void)sub; + + if (info.status == MATCHED_MATCHING) { + n_matched++; - if (info.status == MATCHED_MATCHING) { - n_matched++; - } else { - n_matched--; - } + } else { + n_matched--; + } @[end if]@ } -void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub) +void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber *sub) { - if (n_matched > 0) { - std::unique_lock has_msg_lock(has_msg_mutex); - if(has_msg.load() == true) // Check if msg has been fetched - { - has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched - } - has_msg_lock.unlock(); - - // Take data - if(sub->takeNextData(&msg, &m_info)) - { - if(m_info.sampleKind == ALIVE) - { - std::unique_lock lk(*t_send_queue_mutex); - - ++n_msg; - has_msg = true; - - t_send_queue->push(topic_ID); - lk.unlock(); - t_send_queue_cv->notify_one(); - - } - } - } + if (n_matched > 0) { + std::unique_lock has_msg_lock(has_msg_mutex); + + if (has_msg.load() == true) { // Check if msg has been fetched + has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched + } + + has_msg_lock.unlock(); + + // Take data + if (sub->takeNextData(&msg, &m_info)) { + if (m_info.sampleKind == ALIVE) { + std::unique_lock lk(*t_send_queue_mutex); + + ++n_msg; + has_msg = true; + + t_send_queue->push(topic_ID); + lk.unlock(); + t_send_queue_cv->notify_one(); + + } + } + } } bool @(topic)_Subscriber::hasMsg() { - if (m_listener.n_matched > 0) { - return m_listener.has_msg.load(); - } + if (m_listener.n_matched > 0) { + return m_listener.has_msg.load(); + } - return false; + return false; } @(topic)_msg_t @(topic)_Subscriber::getMsg() { - return m_listener.msg; + return m_listener.msg; } void @(topic)_Subscriber::unlockMsg() { - if (m_listener.n_matched > 0) { - std::unique_lock has_msg_lock(m_listener.has_msg_mutex); - m_listener.has_msg = false; - has_msg_lock.unlock(); - m_listener.has_msg_cv.notify_one(); - } + if (m_listener.n_matched > 0) { + std::unique_lock has_msg_lock(m_listener.has_msg_mutex); + m_listener.has_msg = false; + has_msg_lock.unlock(); + m_listener.has_msg_cv.notify_one(); + } } diff --git a/msg/templates/urtps/Subscriber.h.em b/msg/templates/urtps/Subscriber.h.em index c24e32dccaf0..703112bef3f8 100644 --- a/msg/templates/urtps/Subscriber.h.em +++ b/msg/templates/urtps/Subscriber.h.em @@ -6,15 +6,13 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - ros2_distro (str) ROS2 distro name +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @############################################### @{ -from packaging import version import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +from packaging import version +import re topic = alias if alias else spec.short_name try: @@ -25,7 +23,7 @@ except AttributeError: /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -59,7 +57,7 @@ except AttributeError: * @@file @(topic)_Subscriber.h * This header file contains the declaration of the subscriber functions. * - * This file was adapted from the fastcdrgen tool. + * This file was adapted from the fastrtpsgen tool. */ @@ -103,39 +101,40 @@ using @(topic)_msg_datatype = @(topic)PubSubType; class @(topic)_Subscriber { public: - @(topic)_Subscriber(); - virtual ~@(topic)_Subscriber(); - bool init(uint8_t topic_ID, std::condition_variable* t_send_queue_cv, std::mutex* t_send_queue_mutex, std::queue* t_send_queue, const std::string& ns); - void run(); - bool hasMsg(); - @(topic)_msg_t getMsg(); - void unlockMsg(); + @(topic)_Subscriber(); + virtual ~@(topic)_Subscriber(); + bool init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, + std::queue *t_send_queue, const std::string &ns, std::string topic_name = ""); + void run(); + bool hasMsg(); + @(topic)_msg_t getMsg(); + void unlockMsg(); private: - Participant *mp_participant; - Subscriber *mp_subscriber; + Participant *mp_participant; + Subscriber *mp_subscriber; - class SubListener : public SubscriberListener - { - public: - SubListener() : n_matched(0), n_msg(0), has_msg(false){}; - ~SubListener(){}; - void onSubscriptionMatched(Subscriber* sub, MatchingInfo& info); - void onNewDataMessage(Subscriber* sub); - SampleInfo_t m_info; - int n_matched; - int n_msg; - @(topic)_msg_t msg; - std::atomic_bool has_msg; - uint8_t topic_ID; - std::condition_variable* t_send_queue_cv; - std::mutex* t_send_queue_mutex; - std::queue* t_send_queue; - std::condition_variable has_msg_cv; - std::mutex has_msg_mutex; + class SubListener : public SubscriberListener + { + public: + SubListener() : n_matched(0), n_msg(0), has_msg(false) {}; + ~SubListener() {}; + void onSubscriptionMatched(Subscriber *sub, MatchingInfo &info); + void onNewDataMessage(Subscriber *sub); + SampleInfo_t m_info; + int n_matched; + int n_msg; + @(topic)_msg_t msg; + std::atomic_bool has_msg; + uint8_t topic_ID; + std::condition_variable *t_send_queue_cv; + std::mutex *t_send_queue_mutex; + std::queue *t_send_queue; + std::condition_variable has_msg_cv; + std::mutex has_msg_mutex; - } m_listener; - @(topic)_msg_datatype @(topic)DataType; + } m_listener; + @(topic)_msg_datatype @(topic)DataType; }; #endif // _@(topic)__SUBSCRIBER_H_ diff --git a/msg/templates/urtps/microRTPS_agent.cpp.em b/msg/templates/urtps/microRTPS_agent.cpp.em index 728eb2a9d22b..07510f917427 100644 --- a/msg/templates/urtps/microRTPS_agent.cpp.em +++ b/msg/templates/urtps/microRTPS_agent.cpp.em @@ -6,23 +6,25 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file +@# - ros2_distro (List[str]) ROS2 distro name @############################################### @{ import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ +try: + ros2_distro = ros2_distro[0].decode("utf-8") +except AttributeError: + ros2_distro = ros2_distro[0] + send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND] recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE] }@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -54,6 +56,7 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer #include #include +#include #include #include #include @@ -72,108 +75,151 @@ recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumer #include "microRTPS_timesync.h" #include "RtpsTopics.h" +@[if ros2_distro]@ +#include +@[end if]@ + // Default values -#define DEVICE "/dev/ttyACM0" -#define SLEEP_US 1 -#define BAUDRATE 460800 -#define POLL_MS 1 -#define WAIT_CNST 2 +#define SLEEP_US 1 +#define MAX_SLEEP_US 1000000 +#define BAUDRATE 460800 +#define MAX_DATA_RATE 10000000 +#define DEVICE "/dev/ttyACM0" +#define POLL_MS 1 +#define MAX_POLL_MS 1000 #define DEFAULT_RECV_PORT 2020 #define DEFAULT_SEND_PORT 2019 -#define DEFAULT_IP "127.0.0.1" +#define DEFAULT_IP "127.0.0.1" using namespace eprosima; using namespace eprosima::fastrtps; volatile sig_atomic_t running = 1; -Transport_node *transport_node = nullptr; -RtpsTopics topics; +std::unique_ptr transport_node; +std::unique_ptr topics; uint32_t total_sent = 0, sent = 0; struct options { - enum class eTransports - { - UART, - UDP - }; - eTransports transport = options::eTransports::UART; - char device[64] = DEVICE; - int sleep_us = SLEEP_US; - uint32_t baudrate = BAUDRATE; - int poll_ms = POLL_MS; - uint16_t recv_port = DEFAULT_RECV_PORT; - uint16_t send_port = DEFAULT_SEND_PORT; - char ip[16] = DEFAULT_IP; - bool sw_flow_control = false; - bool hw_flow_control = false; - bool verbose_debug = false; - std::string ns = ""; + enum class eTransports { + UART, + UDP + }; + eTransports transport = options::eTransports::UART; + char device[64] = DEVICE; + int sleep_us = SLEEP_US; + uint32_t baudrate = BAUDRATE; + int poll_ms = POLL_MS; + uint16_t recv_port = DEFAULT_RECV_PORT; + uint16_t send_port = DEFAULT_SEND_PORT; + char ip[16] = DEFAULT_IP; + bool sw_flow_control = false; + bool hw_flow_control = false; + bool verbose_debug = false; + std::string ns = ""; } _options; static void usage(const char *name) { - printf("usage: %s [options]\n\n" - " -b UART device baudrate. Default 460800\n" - " -d UART device. Default /dev/ttyACM0\n" - " -f Activates UART link SW flow control\n" - " -h Activates UART link HW flow control\n" - " -i Target IP for UDP. Default 127.0.0.1\n" - " -n ROS 2 topics namespace. Identifies the vehicle in a multi-agent network\n" - " -p Time in ms to poll over UART. Default 1ms\n" - " -r UDP port for receiving. Default 2019\n" - " -s UDP port for sending. Default 2020\n" - " -t [UART|UDP] Default UART\n" - " -v Add more verbosity\n" - " -w Time in us for which each iteration sleep. Default 1ms\n", - name); + printf("usage: %s [options]\n\n" + " -b UART device baudrate. Defaults to 460800\n" + " -d UART device. Defaults to /dev/ttyACM0\n" + " -f Activates UART link SW flow control\n" + " -g Activates UART link HW flow control\n" + " -i Target remote IP address for UDP. Defaults to 127.0.0.1\n" + " -n Topics namespace. Identifies the vehicle in a multi-agent network\n" + " -o UART polling timeout in milliseconds. Defaults to 1ms\n" + " -r UDP port for receiving (local). Defaults to 2020\n" + " -s UDP port for sending (remote). Defaults to 2019\n" + " -t [UART|UDP] Defaults to UART\n" + " -v Add more verbosity\n" + " -w Iteration time for data publishing to the DDS world, in microseconds.\n" + " Defaults to 1us\n" + " (ROS2 only) Allows to pass arguments to the timesync ROS2 node.\n" + " Currently used for setting the usage of simulation time by the node using\n" + " '--ros-args -p use_sim_time:=true'\n", + name); } static int parse_options(int argc, char **argv) { - int ch; - - while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:i:fhvn:")) != EOF) - { - switch (ch) - { - case 't': _options.transport = strcmp(optarg, "UDP") == 0? - options::eTransports::UDP - :options::eTransports::UART; break; - case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break; - case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break; - case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; - case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break; - case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; - case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; - case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break; - case 'f': _options.sw_flow_control = true; break; - case 'h': _options.hw_flow_control = true; break; - case 'v': _options.verbose_debug = true; break; - case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break; - default: - usage(argv[0]); - return -1; - } - } - - if (_options.poll_ms < 1) { - _options.poll_ms = 1; - printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low, using 1 ms\033[0m"); - } - - if (_options.hw_flow_control && _options.sw_flow_control) { - printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m"); - return -1; - } - - return 0; -} + static const struct option options[] = { + {"baudrate", required_argument, NULL, 'b'}, + {"device", required_argument, NULL, 'd'}, + {"sw-flow-control", no_argument, NULL, 'f'}, + {"hw-flow-control", no_argument, NULL, 'g'}, + {"ip-address", required_argument, NULL, 'i'}, + {"namespace", required_argument, NULL, 'n'}, + {"poll-ms", required_argument, NULL, 'o'}, + {"reception-port", required_argument, NULL, 'r'}, + {"sending-port", required_argument, NULL, 's'}, + {"transport", required_argument, NULL, 't'}, + {"increase-verbosity", no_argument, NULL, 'v'}, + {"sleep-time-us", required_argument, NULL, 'w'}, + {"ros-args", required_argument, NULL, 0}, + {"help", no_argument, NULL, 'h'}, + {NULL, 0, NULL, 0}}; -void signal_handler(int signum) -{ - printf("\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); - running = 0; - transport_node->close(); + int ch; + + while ((ch = getopt_long(argc, argv, "t:d:w:b:o:r:s:i:fghvn:", options, nullptr)) >= 0) { + switch (ch) { + case 't': _options.transport = strcmp(optarg, "UDP") == 0 ? + options::eTransports::UDP + : options::eTransports::UART; break; + + case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break; + + case 'w': _options.sleep_us = strtoul(optarg, nullptr, 10); break; + + case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break; + + case 'o': _options.poll_ms = strtol(optarg, nullptr, 10); break; + + case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break; + + case 's': _options.send_port = strtoul(optarg, nullptr, 10); break; + + case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break; + + case 'f': _options.sw_flow_control = true; break; + + case 'g': _options.hw_flow_control = true; break; + + case 'h': usage(argv[0]); exit(0); break; + + case 'v': _options.verbose_debug = true; break; + + case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break; + + default: +@[if ros2_distro]@ + break; +@[else]@ + usage(argv[0]); + return -1; +@[end if]@ + } + } + + if (_options.poll_ms < POLL_MS) { + _options.poll_ms = POLL_MS; + printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low. Using %d ms instead\033[0m\n", POLL_MS); + } else if (_options.poll_ms > MAX_POLL_MS) { + _options.poll_ms = MAX_POLL_MS; + printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too high. Using %d ms instead\033[0m\n", MAX_POLL_MS); + } + + if (_options.sleep_us > MAX_SLEEP_US) { + _options.sleep_us = MAX_SLEEP_US; + printf("\033[1;33m[ micrortps_agent ]\tPublishing iteration cycle too slow. Using %d us instead\033[0m\n", MAX_SLEEP_US); + } + + if (_options.hw_flow_control && _options.sw_flow_control) { + printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m\n"); + return -1; + } + + return 0; } @[if recv_topics]@ @@ -182,147 +228,177 @@ std::condition_variable t_send_queue_cv; std::mutex t_send_queue_mutex; std::queue t_send_queue; -void t_send(void*) +void t_send(void *) { - char data_buffer[BUFFER_SIZE] = {}; - uint32_t length = 0; - - while (running && !exit_sender_thread.load()) - { - std::unique_lock lk(t_send_queue_mutex); - while (t_send_queue.empty() && !exit_sender_thread.load()) - { - t_send_queue_cv.wait(lk); - } - uint8_t topic_ID = t_send_queue.front(); - t_send_queue.pop(); - lk.unlock(); - - size_t header_length = transport_node->get_header_length(); - /* make room for the header to fill in later */ - eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer)-header_length); - eprosima::fastcdr::Cdr scdr(cdrbuffer); - - if (topics.getMsg(topic_ID, scdr)) - { - length = scdr.getSerializedDataLength(); - if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) - { - total_sent += length; - ++sent; - } - } - } + char data_buffer[BUFFER_SIZE] = {}; + uint32_t length = 0; + uint8_t topic_ID = 255; + + while (running && !exit_sender_thread) { + std::unique_lock lk(t_send_queue_mutex); + + while (t_send_queue.empty() && !exit_sender_thread) { + t_send_queue_cv.wait(lk); + } + + topic_ID = t_send_queue.front(); + t_send_queue.pop(); + lk.unlock(); + + size_t header_length = transport_node->get_header_length(); + /* make room for the header to fill in later */ + eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer) - header_length); + eprosima::fastcdr::Cdr scdr(cdrbuffer); + + if (!exit_sender_thread) { + if (topics->getMsg(topic_ID, scdr)) { + length = scdr.getSerializedDataLength(); + + if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) { + total_sent += length; + ++sent; + } + } + } + } } @[end if]@ -int main(int argc, char** argv) +void signal_handler(int signum) { - if (-1 == parse_options(argc, argv)) - { - printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - // register signal SIGINT and signal handler - signal(SIGINT, signal_handler); - - printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n"); - printf("[ micrortps_agent ]\tStarting link...\n"); - - switch (_options.transport) - { - case options::eTransports::UART: - { - transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms, - _options.sw_flow_control, _options.hw_flow_control, _options.verbose_debug); - printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms; flow_control: %s\n", - _options.device, _options.baudrate, _options.sleep_us, _options.poll_ms, - _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); - } - break; - case options::eTransports::UDP: - { - transport_node = new UDP_node(_options.ip, _options.recv_port, _options.send_port, _options.verbose_debug); - printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u; sleep: %dus\n", - _options.ip, _options.recv_port, _options.send_port, _options.sleep_us); - } - break; - default: - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - if (0 > transport_node->init()) - { - printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); - return -1; - } - - sleep(1); + printf("\n\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum); + running = 0; + transport_node->close(); +} -@[if send_topics]@ - char data_buffer[BUFFER_SIZE] = {}; - int received = 0, loop = 0; - int length = 0, total_read = 0; - bool receiving = false; - uint8_t topic_ID = 255; - std::chrono::time_point start, end; +int main(int argc, char **argv) +{ + printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n"); + + if (-1 == parse_options(argc, argv)) { + printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n"); + return -1; + } + + topics = std::make_unique(); + +@[if ros2_distro]@ + // Initialize communications via the rmw implementation and set up a global signal handler. + rclcpp::init(argc, argv, rclcpp::InitOptions()); @[end if]@ - // Init timesync - std::shared_ptr timeSync = std::make_shared(_options.verbose_debug); + // register signal SIGINT and signal handler + signal(SIGINT, signal_handler); + + printf("[ micrortps_agent ]\tStarting link...\n"); + + const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY"); + + if (localhost_only && strcmp(localhost_only, "1") == 0) { + printf("[ micrortps_agent ]\tUsing only the localhost network...\n"); + } + + /** + * Set the system ID to Mission Computer, in order to identify the agent side + * + * Note: theoretically a multi-agent system is possible, but this would require + * adjustments in the way the timesync is done (would have to create a timesync + * instance per agent). Keeping it contained for a 1:1 link for now is reasonable. + */ + const uint8_t sys_id = static_cast(MicroRtps::System::MISSION_COMPUTER); + + switch (_options.transport) { + case options::eTransports::UART: { + transport_node = std::make_unique(_options.device, _options.baudrate, _options.poll_ms, + _options.sw_flow_control, _options.hw_flow_control, sys_id, _options.verbose_debug); + printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; poll: %dms; flow_control: %s\n", + _options.device, _options.baudrate, _options.poll_ms, + _options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No")); + } + break; + + case options::eTransports::UDP: { + transport_node = std::make_unique(_options.ip, _options.recv_port, _options.send_port, + sys_id, _options.verbose_debug); + printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u\n", + _options.ip, _options.recv_port, _options.send_port); + } + break; + + default: + printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); + return -1; + } + + if (0 > transport_node->init()) { + printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n"); + return -1; + } + + sleep(1); + +@[if send_topics]@ + char data_buffer[BUFFER_SIZE] = {}; + int received = 0, loop = 0; + int length = 0, total_read = 0; + bool receiving = false; + uint8_t topic_ID = 255; + std::chrono::time_point start, end; +@[end if]@ - topics.set_timesync(timeSync); + // Init timesync + topics->set_timesync(std::make_shared(_options.verbose_debug)); @[if recv_topics]@ - topics.init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns); + topics->init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns); @[end if]@ - running = true; + running = true; @[if recv_topics]@ - std::thread sender_thread(t_send, nullptr); + std::thread sender_thread(t_send, nullptr); @[end if]@ - while (running) - { + while (running) { @[if send_topics]@ - ++loop; - if (!receiving) start = std::chrono::steady_clock::now(); - // Publish messages received from UART - while (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) - { - topics.publish(topic_ID, data_buffer, sizeof(data_buffer)); - ++received; - total_read += length; - receiving = true; - end = std::chrono::steady_clock::now(); - } - - if ((receiving && std::chrono::duration(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) || - (!running && loop > 1)) - { - std::chrono::duration elapsed_secs = end - start; - printf("[ micrortps_agent ]\tSENT: %lumessages \t- %lubytes\n", (unsigned long)sent, (unsigned long)total_sent); - printf("[ micrortps_agent ]\tRECEIVED: %dmessages \t- %dbytes; %d LOOPS - %.03f seconds - %.02fKB/s\n", - received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count())); - received = sent = total_read = total_sent = 0; - receiving = false; - } + ++loop; + if (!receiving) { start = std::chrono::steady_clock::now(); } + + // Publish messages received from UART + if (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) { + topics->publish(topic_ID, data_buffer, sizeof(data_buffer)); + ++received; + total_read += length; + receiving = true; + end = std::chrono::steady_clock::now(); + } @[else]@ - usleep(_options.sleep_us); + usleep(_options.sleep_us); +@[end if]@ + } + +@[if recv_topics]@ + exit_sender_thread = true; + t_send_queue_cv.notify_one(); + sender_thread.join(); + + std::chrono::duration elapsed_secs = end - start; + if (received > 0) { + printf("[ micrortps_agent ]\tRECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n", + received, total_read, loop, elapsed_secs.count(), static_cast(total_read) / (1000 * elapsed_secs.count())); + } @[end if]@ - } @[if recv_topics]@ - exit_sender_thread = true; - t_send_queue_cv.notify_one(); - sender_thread.join(); + if (sent > 0) { + printf("[ micrortps_agent ]\tSENT: %lu messages - %lu bytes\n", static_cast(sent), + static_cast(total_sent)); + } @[end if]@ - delete transport_node; - transport_node = nullptr; - timeSync->stop(); - timeSync->reset(); +@[if ros2_distro]@ + rclcpp::shutdown(); +@[end if]@ + transport_node.reset(); + topics.reset(); - return 0; + return 0; } diff --git a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em index 2f642734f3a0..f75d13f5d179 100644 --- a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em +++ b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em @@ -1,7 +1,7 @@ ################################################################################ # # Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-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: @@ -42,14 +42,22 @@ find_package(fastcdr REQUIRED) include(CheckCXXCompilerFlag) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - check_cxx_compiler_flag(--std=c++14 SUPPORTS_CXX14) - if(SUPPORTS_CXX14) - add_compile_options(--std=c++14) - else() - message(FATAL_ERROR "Compiler doesn't support C++14") - endif() + check_cxx_compiler_flag(--std=c++14 SUPPORTS_CXX14) + if(SUPPORTS_CXX14) + add_compile_options(--std=c++14) + else() + message(FATAL_ERROR "Compiler doesn't support C++14") + endif() endif() file(GLOB MICRORTPS_AGENT_SOURCES src/*.cpp src/*.h) add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES}) target_link_libraries(micrortps_agent fastrtps fastcdr) + +# Install to '/usr/local/bin' if `make install` is used +install( + TARGETS micrortps_agent + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/msg/templates/urtps/microRTPS_timesync.cpp.em b/msg/templates/urtps/microRTPS_timesync.cpp.em index ba5d9bab3aef..a441339fb1d6 100644 --- a/msg/templates/urtps/microRTPS_timesync.cpp.em +++ b/msg/templates/urtps/microRTPS_timesync.cpp.em @@ -6,14 +6,11 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - package (List[str]) messages package name. Defaulted to 'px4' +@# - ros2_distro (List[str]) ROS2 distro name @############################################### @{ import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ from px_generate_uorb_topic_files import MsgScope # this is in Tools/ package = package[0] @@ -25,7 +22,7 @@ except AttributeError: }@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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: @@ -67,87 +64,140 @@ except AttributeError: #include "microRTPS_timesync.h" TimeSync::TimeSync(bool debug) - : _offset_ns(-1), - _skew_ns_per_sync(0.0), - _num_samples(0), - _request_reset_counter(0), - _last_msg_seq(0), - _last_remote_msg_seq(0), - _debug(debug) + : _offset_ns(-1), +@[if ros2_distro]@ + _timesync_node(std::make_shared("timesync_node")), +@[end if]@ + _skew_ns_per_sync(0.0), + _num_samples(0), + _request_reset_counter(0), + _last_msg_seq(0), + _last_remote_msg_seq(0), + _debug(debug) { } TimeSync::~TimeSync() { stop(); } -void TimeSync::start(const TimesyncPublisher* pub) { +void TimeSync::start(TimesyncPublisher *pub) +{ stop(); - _timesync_pub = (*pub); +@[if ros2_distro]@ + auto spin_node = [this]() { + rclcpp::spin(_timesync_node); + }; +@[end if]@ - auto run = [this]() { + auto run = [this, pub]() { while (!_request_stop) { timesync_msg_t msg = newTimesyncMsg(); - _timesync_pub.publish(&msg); + pub->publish(&msg); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } }; _request_stop = false; +@[if ros2_distro]@ + _timesync_node_thread.reset(new std::thread(spin_node)); +@[end if]@ _send_timesync_thread.reset(new std::thread(run)); } -void TimeSync::stop() { +void TimeSync::init_status_pub(TimesyncStatusPublisher *status_pub) +{ + auto run = [this, status_pub]() { + while (!_request_stop) { + timesync_status_msg_t status_msg = newTimesyncStatusMsg(); + + status_pub->publish(&status_msg); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + }; + _request_stop = false; + _send_timesync_status_thread.reset(new std::thread(run)); +} + +void TimeSync::stop() +{ _request_stop = true; - if (_send_timesync_thread && _send_timesync_thread->joinable()) _send_timesync_thread->join(); - _send_timesync_thread.reset(); + +@[if ros2_distro]@ + if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); } +@[end if]@ + if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); } + if (_send_timesync_status_thread && _send_timesync_status_thread->joinable()) { _send_timesync_status_thread->join(); } } -void TimeSync::reset() { +void TimeSync::reset() +{ _num_samples = 0; _request_reset_counter = 0; } -int64_t TimeSync::getTimeNSec() { +@[if ros2_distro]@ +uint64_t TimeSync::getROSTimeNSec() const +{ + return _timesync_node->now().nanoseconds(); +} + +uint64_t TimeSync::getROSTimeUSec() const +{ + return RCL_NS_TO_US(getROSTimeNSec()); +} +@[else]@ +uint64_t TimeSync::getSteadyTimeNSec() const +{ auto time = std::chrono::steady_clock::now(); return std::chrono::time_point_cast(time).time_since_epoch().count(); } -int64_t TimeSync::getTimeUSec() { +uint64_t TimeSync::getSteadyTimeUSec() const +{ auto time = std::chrono::steady_clock::now(); return std::chrono::time_point_cast(time).time_since_epoch().count(); } +@[end if]@ -bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns) { - int64_t rtti = local_t3_ns - local_t1_ns; +bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns) +{ + _rtti = local_t3_ns - local_t1_ns; + _remote_time_stamp = remote_t2_ns; // assume rtti is evenly split both directions - int64_t remote_t3_ns = remote_t2_ns + rtti / 2ll; + int64_t remote_t3_ns = remote_t2_ns + _rtti.load() / 2ll; int64_t measurement_offset = remote_t3_ns - local_t3_ns; if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) { reset(); - if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl; + + if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl; } } - if (_num_samples == 0) { - updateOffset(measurement_offset); - _skew_ns_per_sync = 0; - } + if (_num_samples == 0) { + updateOffset(measurement_offset); + _skew_ns_per_sync = 0; + } if (_num_samples >= WINDOW_SIZE) { if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) { _request_reset_counter++; - if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl; + + if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl; } + return false; + } else { _request_reset_counter = 0; } } // ignore if rtti > 50ms - if (rtti > 50ll * 1000ll * 1000ll) { - if (_debug) std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << rtti / (1000ll * 1000ll) << "ms\033[0m" << std::endl; + if (_rtti.load() > 50ll * 1000ll * 1000ll) { + if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << _rtti.load() / (1000ll * 1000ll) << "ms\033[0m" << std::endl; } + return false; } @@ -161,47 +211,85 @@ bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL; } - int64_t offset_prev = _offset_ns.load(); + _offset_prev = _offset_ns.load(); updateOffset(static_cast((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) + measurement_offset * alpha)); _skew_ns_per_sync = - static_cast(beta * (_offset_ns.load() - offset_prev) + (1. - beta) * _skew_ns_per_sync); + static_cast(beta * (_offset_ns.load() - _offset_prev.load()) + (1. - beta) * _skew_ns_per_sync); _num_samples++; return true; } -void TimeSync::processTimesyncMsg(timesync_msg_t * msg) { - if (getMsgSysID(msg) == 1 && getMsgSeq(msg) != _last_remote_msg_seq) { - _last_remote_msg_seq = getMsgSeq(msg); +void TimeSync::processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub) +{ + if (getMsgSeq(msg) != _last_remote_msg_seq) { + _last_remote_msg_seq = getMsgSeq(msg); if (getMsgTC1(msg) > 0) { - if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getTimeNSec())) { - if (_debug) std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; +@[if ros2_distro]@ + if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getROSTimeNSec())) { +@[else]@ + if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getSteadyTimeNSec())) { +@[end if]@ + if (_debug) { std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; } } } else if (getMsgTC1(msg) == 0) { - setMsgTimestamp(msg, getTimeUSec()); - setMsgSysID(msg, 0); +@[if ros2_distro]@ + setMsgTimestamp(msg, getROSTimeUSec()); +@[else]@ + setMsgTimestamp(msg, getSteadyTimeUSec()); +@[end if]@ setMsgSeq(msg, getMsgSeq(msg) + 1); - setMsgTC1(msg, getTimeNSec()); +@[if ros2_distro]@ + setMsgTC1(msg, getROSTimeNSec()); +@[else]@ + setMsgTC1(msg, getSteadyTimeNSec()); +@[end if]@ - _timesync_pub.publish(msg); + pub->publish(msg); } } } -timesync_msg_t TimeSync::newTimesyncMsg() { +timesync_msg_t TimeSync::newTimesyncMsg() +{ timesync_msg_t msg{}; - setMsgTimestamp(&msg, getTimeUSec()); - setMsgSysID(&msg, 0); +@[if ros2_distro]@ + setMsgTimestamp(&msg, getROSTimeUSec()); +@[else]@ + setMsgTimestamp(&msg, getSteadyTimeUSec()); +@[end if]@ setMsgSeq(&msg, _last_msg_seq); setMsgTC1(&msg, 0); - setMsgTS1(&msg, getTimeNSec()); +@[if ros2_distro]@ + setMsgTS1(&msg, getROSTimeNSec()); +@[else]@ + setMsgTS1(&msg, getSteadyTimeNSec()); +@[end if]@ _last_msg_seq++; return msg; } + +timesync_status_msg_t TimeSync::newTimesyncStatusMsg() +{ + timesync_status_msg_t msg{}; + +@[if ros2_distro]@ + setMsgTimestamp(&msg, getROSTimeUSec()); +@[else]@ + setMsgTimestamp(&msg, getSteadyTimeUSec()); +@[end if]@ + setMsgSourceProtocol(&msg, 1); // SOURCE_PROTOCOL_RTPS + setMsgRemoteTimeStamp(&msg, _remote_time_stamp.load() / 1000ULL); + setMsgObservedOffset(&msg, _offset_prev.load()); + setMsgEstimatedOffset(&msg, _offset_ns.load()); + setMsgRoundTripTime(&msg, _rtti.load() / 1000ll); + + return msg; +} diff --git a/msg/templates/urtps/microRTPS_timesync.h.em b/msg/templates/urtps/microRTPS_timesync.h.em index 2a3e12bd8d0a..ffe4541f1854 100644 --- a/msg/templates/urtps/microRTPS_timesync.h.em +++ b/msg/templates/urtps/microRTPS_timesync.h.em @@ -6,15 +6,12 @@ @# Start of Template @# @# Context: -@# - msgs (List) list of all msg files -@# - multi_topics (List) list of all multi-topic names -@# - ids (List) list of all RTPS msg ids +@# - package (List[str]) messages package name. Defaulted to 'px4' +@# - ros2_distro (List[str]) ROS2 distro name @############################################### @{ -from packaging import version import genmsg.msgs - -from px_generate_uorb_topic_helper import * # this is in Tools/ +from packaging import version from px_generate_uorb_topic_files import MsgScope # this is in Tools/ package = package[0] @@ -26,7 +23,7 @@ except AttributeError: }@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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: @@ -71,10 +68,14 @@ except AttributeError: @[if ros2_distro]@ #include "Timesync_Publisher.h" -#include "Timesync_Subscriber.h" +#include "TimesyncStatus_Publisher.h" + +#include +#include +#include @[else]@ #include "timesync_Publisher.h" -#include "timesync_Subscriber.h" +#include "timesync_status_Publisher.h" @[end if]@ static constexpr double ALPHA_INITIAL = 0.05; @@ -90,24 +91,31 @@ static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5; @[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@ @[ if ros2_distro]@ using timesync_msg_t = @(package)::msg::dds_::Timesync_; +using timesync_status_msg_t = @(package)::msg::dds_::TimesyncStatus_; @[ else]@ using timesync_msg_t = timesync_; +using timesync_status_msg_t = timesync_status_; @[ end if]@ @[else]@ @[ if ros2_distro]@ using timesync_msg_t = @(package)::msg::Timesync; +using timesync_status_msg_t = @(package)::msg::TimesyncStatus; @[ else]@ using timesync_msg_t = timesync; +using timesync_status_msg_t = timesync_status; @[ end if]@ @[end if]@ @# Sets the timesync publisher entity depending on using ROS2 or not @[if ros2_distro]@ using TimesyncPublisher = Timesync_Publisher; +using TimesyncStatusPublisher = TimesyncStatus_Publisher; @[else]@ using TimesyncPublisher = timesync_Publisher; +using TimesyncStatusPublisher = timesync_status_Publisher; @[end if]@ -class TimeSync { +class TimeSync +{ public: TimeSync(bool debug); virtual ~TimeSync(); @@ -116,7 +124,13 @@ public: * @@brief Starts the timesync publishing thread * @@param[in] pub The timesync publisher entity to use */ - void start(const TimesyncPublisher* pub); + void start(TimesyncPublisher *pub); + + /** + * @@brief Init and run the timesync status publisher thread + * @@param[in] pub The timesync status publisher entity to use + */ + void init_status_pub(TimesyncStatusPublisher *status_pub); /** * @@brief Resets the filter @@ -128,17 +142,36 @@ public: */ void stop(); +@[if ros2_distro]@ + /** + * @@brief Get ROS time in nanoseconds. This will match the system time, which + * corresponds to the system-wide real time since epoch. If use_sim_time + * is set, the simulation time is grabbed by the node and used instead + * More info about ROS2 clock and time in: + * https://design.ros2.org/articles/clock_and_time.html + * @@return ROS time in nanoseconds + */ + uint64_t getROSTimeNSec() const; + + /** + * @@brief Get ROS time in microseconds. Fetches the time from getROSTimeNSec() + * and converts it to microseconds + * @@return ROS time in microseconds + */ + uint64_t getROSTimeUSec() const; +@[else]@ /** * @@brief Get clock monotonic time (raw) in nanoseconds - * @@return System CLOCK_MONOTONIC time in nanoseconds + * @@return Steady CLOCK_MONOTONIC time in nanoseconds */ - static int64_t getTimeNSec(); + uint64_t getSteadyTimeNSec() const; /** - * @@brief Get system monotonic time in microseconds - * @@return System CLOCK_MONOTONIC time in microseconds + * @@brief Get clock monotonic time (raw) in microseconds + * @@return Steady CLOCK_MONOTONIC time in microseconds */ - static int64_t getTimeUSec(); + uint64_t getSteadyTimeUSec() const; +@[end if]@ /** * @@brief Adds a time offset measurement to be filtered @@ -153,7 +186,7 @@ public: * @@brief Processes DDS timesync message * @@param[in,out] msg The timestamp msg to be processed */ - void processTimesyncMsg(timesync_msg_t* msg); + void processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub); /** * @@brief Creates a new timesync DDS message to be sent from the agent to the client @@ -161,6 +194,12 @@ public: */ timesync_msg_t newTimesyncMsg(); + /** + * @@brief Creates a new timesync status DDS message to be sent from the agent to the client + * @@return A new timesync status message with the origin in the agent and with the agent timestamp + */ + timesync_status_msg_t newTimesyncStatusMsg(); + /** * @@brief Get the time sync offset in nanoseconds * @@return The offset in nanoseconds @@ -171,16 +210,27 @@ public: * @@brief Sums the time sync offset to the timestamp * @@param[in,out] timestamp The timestamp to add the offset to */ - inline void addOffset(uint64_t& timestamp) { timestamp = (timestamp * 1000LL + _offset_ns.load()) / 1000ULL; } + inline void addOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL + _offset_ns.load()) / 1000ULL; } /** * @@brief Substracts the time sync offset to the timestamp * @@param[in,out] timestamp The timestamp to subtract the offset of */ - inline void subtractOffset(uint64_t& timestamp) { timestamp = (timestamp * 1000LL - _offset_ns.load()) / 1000ULL; } + inline void subtractOffset(uint64_t ×tamp) { timestamp = (timestamp * 1000LL - _offset_ns.load()) / 1000ULL; } private: std::atomic _offset_ns; + std::atomic _offset_prev; + std::atomic _remote_time_stamp; + std::atomic _rtti; + +@[if ros2_distro]@ + /** + * @@brief A ROS2 node to fetch the ROS time to be used for timesync + */ + std::shared_ptr _timesync_node; +@[end if]@ + int64_t _skew_ns_per_sync; int64_t _num_samples; @@ -190,50 +240,64 @@ private: bool _debug; + std::unique_ptr _send_timesync_thread; + std::unique_ptr _send_timesync_status_thread; @[if ros2_distro]@ - Timesync_Publisher _timesync_pub; - Timesync_Subscriber _timesync_sub; -@[else]@ - timesync_Publisher _timesync_pub; - timesync_Subscriber _timesync_sub; + std::unique_ptr _timesync_node_thread; @[end if]@ - - std::unique_ptr _send_timesync_thread; std::atomic _request_stop{false}; /** * @@brief Updates the offset of the time sync filter * @@param[in] offset The value of the offset to update to */ - inline void updateOffset(const uint64_t& offset) { _offset_ns.store(offset, std::memory_order_relaxed); } + inline void updateOffset(const uint64_t &offset) { _offset_ns.store(offset, std::memory_order_relaxed); } /** Timesync msg Getters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp_(); } - inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id_(); } - inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq_(); } - inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1_(); } - inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1_(); } + inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp_(); } + inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq_(); } + inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1_(); } + inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1_(); } +@[elif ros2_distro]@ + inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp(); } + inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq(); } + inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1(); } + inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1(); } + @[end if]@ + + /** Common timestamp setter **/ +@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ + template + inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp_() = timestamp; } @[elif ros2_distro]@ - inline uint64_t getMsgTimestamp(const timesync_msg_t* msg) { return msg->timestamp(); } - inline uint8_t getMsgSysID(const timesync_msg_t* msg) { return msg->sys_id(); } - inline uint8_t getMsgSeq(const timesync_msg_t* msg) { return msg->seq(); } - inline int64_t getMsgTC1(const timesync_msg_t* msg) { return msg->tc1(); } - inline int64_t getMsgTS1(const timesync_msg_t* msg) { return msg->ts1(); } + template + inline void setMsgTimestamp(T *msg, const uint64_t ×tamp) { msg->timestamp() = timestamp; } @[end if]@ /** Timesync msg Setters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ - inline void setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; } - inline void setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; } - inline void setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq_() = seq; } - inline void setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1_() = tc1; } - inline void setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1_() = ts1; } + inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq_() = seq; } + inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1_() = tc1; } + inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1_() = ts1; } +@[elif ros2_distro]@ + inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq() = seq; } + inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1() = tc1; } + inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1() = ts1; } +@[end if]@ + + /** Timesync Status msg Setters **/ +@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ + inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; } + inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; } + inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; } + inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; } + inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; } @[elif ros2_distro]@ - inline void setMsgTimestamp(timesync_msg_t* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; } - inline void setMsgSysID(timesync_msg_t* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; } - inline void setMsgSeq(timesync_msg_t* msg, const uint8_t& seq) { msg->seq() = seq; } - inline void setMsgTC1(timesync_msg_t* msg, const int64_t& tc1) { msg->tc1() = tc1; } - inline void setMsgTS1(timesync_msg_t* msg, const int64_t& ts1) { msg->ts1() = ts1; } + inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; } + inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; } + inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; } + inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; } + inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; } @[end if]@ }; diff --git a/msg/templates/urtps/microRTPS_transport.cpp b/msg/templates/urtps/microRTPS_transport.cpp index 7b01463bda27..9252d26eb933 100644 --- a/msg/templates/urtps/microRTPS_transport.cpp +++ b/msg/templates/urtps/microRTPS_transport.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -36,13 +36,20 @@ #include #include #include +#include +#include #if __has_include("px4_platform_common/log.h") && __has_include("px4_platform_common/time.h") #include #include #endif +#if defined(__linux__) || defined(__PX4_LINUX) +#include +#endif /* __linux__ */ + #include "microRTPS_transport.h" + /** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */ uint16_t const crc16_table[256] = { 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, @@ -79,9 +86,10 @@ uint16_t const crc16_table[256] = { 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040 }; -Transport_node::Transport_node(const bool _debug): - rx_buff_pos(0), - debug(_debug) +Transport_node::Transport_node(const uint8_t sys_id, const bool debug): + _rx_buff_pos(0), + _debug(debug), + _sys_id(sys_id) { } @@ -105,117 +113,144 @@ uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len) return crc; } -ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer_len) +ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer_len) { - if (nullptr == out_buffer || nullptr == topic_ID || !fds_OK()) { + if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) { return -1; } - *topic_ID = 255; + *topic_id = 255; - ssize_t len = node_read((void *)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos); + ssize_t len = node_read((void *)(_rx_buffer + _rx_buff_pos), sizeof(_rx_buffer) - _rx_buff_pos); if (len < 0) { int errsv = errno; if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) { #ifndef PX4_DEBUG - if (debug) printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); + + if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); } + #else - if (debug) PX4_DEBUG("Read fail %d", errsv); + + if (_debug) { PX4_DEBUG("Read fail %d", errsv); } + #endif /* PX4_DEBUG */ } return len; } - rx_buff_pos += len; + _rx_buff_pos += len; // We read some size_t header_size = sizeof(struct Header); // but not enough - if (rx_buff_pos < header_size) { + if (_rx_buff_pos < header_size) { return 0; } uint32_t msg_start_pos = 0; - for (msg_start_pos = 0; msg_start_pos <= rx_buff_pos - header_size; ++msg_start_pos) { - if ('>' == rx_buffer[msg_start_pos] && memcmp(rx_buffer + msg_start_pos, ">>>", 3) == 0) { + for (msg_start_pos = 0; msg_start_pos <= _rx_buff_pos - header_size; ++msg_start_pos) { + if ('>' == _rx_buffer[msg_start_pos] && memcmp(_rx_buffer + msg_start_pos, ">>>", 3) == 0) { break; } } // Start not found - if (msg_start_pos > (rx_buff_pos - header_size)) { + if (msg_start_pos > (_rx_buff_pos - header_size)) { #ifndef PX4_DEBUG - if (debug) printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %u)\033[0m\n", msg_start_pos); + + if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %" PRIu32 ")\033[0m\n", msg_start_pos); } + #else - if (debug) PX4_DEBUG(" (↓↓ %u)", msg_start_pos); + + if (_debug) { PX4_DEBUG(" (↓↓ %" PRIu32 ")", msg_start_pos); } + #endif /* PX4_DEBUG */ // All we've checked so far is garbage, drop it - but save unchecked bytes - memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); - rx_buff_pos -= msg_start_pos; + memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos); + _rx_buff_pos -= msg_start_pos; return -1; } - // [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] - struct Header *header = (struct Header *)&rx_buffer[msg_start_pos]; + // [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd] + struct Header *header = (struct Header *)&_rx_buffer[msg_start_pos]; uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l; + // The received message comes from this system. Discard it. + // This might happen when: + // 1. The same UDP port is being used to send a rcv packets or + // 2. The same topic on the agent is being used for outgoing and incoming data + if (header->sys_id == _sys_id) { + // Drop the message and continue with the read buffer + memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1)); + _rx_buff_pos -= (msg_start_pos + 1); + return -1; + } + // The message won't fit the buffer. if (buffer_len < header_size + payload_len) { // Drop the message and continue with the read buffer - memmove(rx_buffer, rx_buffer + msg_start_pos + 1, rx_buff_pos - (msg_start_pos + 1)); - rx_buff_pos -= (msg_start_pos + 1); + memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1)); + _rx_buff_pos -= (msg_start_pos + 1); return -EMSGSIZE; } // We do not have a complete message yet - if (msg_start_pos + header_size + payload_len > rx_buff_pos) { + if (msg_start_pos + header_size + payload_len > _rx_buff_pos) { // If there's garbage at the beginning, drop it if (msg_start_pos > 0) { #ifndef PX4_DEBUG - if (debug) printf("\033[1;33m[ micrortps_transport ]\t (↓ %u)\033[0m\n", msg_start_pos); + + if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t (↓ %" PRIu32 ")\033[0m\n", msg_start_pos); } + #else - if (debug) PX4_DEBUG(" (↓ %u)", msg_start_pos); + + if (_debug) { PX4_DEBUG(" (↓ %" PRIu32 ")", msg_start_pos); } + #endif /* PX4_DEBUG */ - memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos); - rx_buff_pos -= msg_start_pos; + memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos); + _rx_buff_pos -= msg_start_pos; } return 0; } uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l; - uint16_t calc_crc = crc16((uint8_t *)rx_buffer + msg_start_pos + header_size, payload_len); + uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + msg_start_pos + header_size, payload_len); if (read_crc != calc_crc) { #ifndef PX4_DEBUG - if (debug) printf("\033[0;31m[ micrortps_transport ]\tBad CRC %u != %u\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); + + if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tBad CRC %" PRIu16 " != %" PRIu16 "\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } + #else - if (debug) PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); + + if (_debug) { PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); } + #endif /* PX4_DEBUG */ // Drop garbage up just beyond the start of the message - memmove(rx_buffer, rx_buffer + (msg_start_pos + 1), rx_buff_pos); + memmove(_rx_buffer, _rx_buffer + (msg_start_pos + 1), _rx_buff_pos); // If there is a CRC error, the payload len cannot be trusted - rx_buff_pos -= (msg_start_pos + 1); + _rx_buff_pos -= (msg_start_pos + 1); len = -1; } else { // copy message to outbuffer and set other return values - memmove(out_buffer, rx_buffer + msg_start_pos + header_size, payload_len); - *topic_ID = header->topic_ID; + memmove(out_buffer, _rx_buffer + msg_start_pos + header_size, payload_len); + *topic_id = header->topic_id; len = payload_len + header_size; - // discard message from rx_buffer - rx_buff_pos -= msg_start_pos + header_size + payload_len; - memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos); + // discard message from _rx_buffer + _rx_buff_pos -= msg_start_pos + header_size + payload_len; + memmove(_rx_buffer, _rx_buffer + msg_start_pos + header_size + payload_len, _rx_buff_pos); } return len; @@ -223,21 +258,22 @@ ssize_t Transport_node::read(uint8_t *topic_ID, char out_buffer[], size_t buffer size_t Transport_node::get_header_length() { - return sizeof(struct Header); + return sizeof(struct Header); } -ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t length) +ssize_t Transport_node::write(const uint8_t topic_id, char buffer[], size_t length) { if (!fds_OK()) { return -1; } - static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u}; + static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u, 0u}; - // [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] + // [>,>,>,topic_id,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end] uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length); - header.topic_ID = topic_ID; + header.topic_id = topic_id; + header.sys_id = _sys_id; header.seq = _seq_number++; header.payload_len_h = (length >> 8) & 0xff; header.payload_len_l = length & 0xff; @@ -248,25 +284,28 @@ ssize_t Transport_node::write(const uint8_t topic_ID, char buffer[], size_t leng /* Fill in the header in the same payload buffer to call a single node_write */ memcpy(buffer, &header, sizeof(header)); ssize_t len = node_write(buffer, length + sizeof(header)); + if (len != ssize_t(length + sizeof(header))) { return len; } + return len + sizeof(header); } -UART_node::UART_node(const char *_uart_name, const uint32_t _baudrate, - const uint32_t _poll_ms, const bool _hw_flow_control, - const bool _sw_flow_control, const bool _debug): - Transport_node(_debug), - uart_fd(-1), - baudrate(_baudrate), - poll_ms(_poll_ms), - hw_flow_control(_hw_flow_control), - sw_flow_control(_sw_flow_control) +UART_node::UART_node(const char *uart_name, const uint32_t baudrate, + const uint32_t poll_ms, const bool hw_flow_control, + const bool sw_flow_control, const uint8_t sys_id, + const bool debug): + Transport_node(sys_id, debug), + _uart_fd(-1), + _baudrate(baudrate), + _poll_ms(poll_ms), + _hw_flow_control(hw_flow_control), + _sw_flow_control(sw_flow_control) { - if (nullptr != _uart_name) { - strcpy(uart_name, _uart_name); + if (nullptr != uart_name) { + strcpy(_uart_name, uart_name); } } @@ -278,20 +317,22 @@ UART_node::~UART_node() int UART_node::init() { // Open a serial port - uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); + _uart_fd = open(_uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); - if (uart_fd < 0) { + if (_uart_fd < 0) { #ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", uart_name, errno); + printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", _uart_name, errno); #else - PX4_ERR("UART transport: Failed to open device: %s (%d)", uart_name, errno); + PX4_ERR("UART transport: Failed to open device: %s (%d)", _uart_name, errno); #endif /* PX4_ERR */ return -errno; } // If using shared UART, no need to set it up - if (baudrate == 0) { - return uart_fd; + if (_baudrate == 0) { + _poll_fd[0].fd = _uart_fd; + _poll_fd[0].events = POLLIN; + return _uart_fd; } // Try to set baud rate @@ -299,48 +340,62 @@ int UART_node::init() int termios_state; // Back up the original uart configuration to restore it after exit - if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0) { + if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", uart_name, termios_state, errno); + printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", _uart_name, termios_state, + errno); #else - PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", uart_name, termios_state, errno); + PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", _uart_name, termios_state, errno); #endif /* PX4_ERR */ close(); return -errno_bkp; } - // Set up the UART for non-canonical binary communication: 8 bits, 1 stop bit, no parity. - uart_config.c_iflag &= !(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF); - uart_config.c_iflag |= IGNBRK | IGNPAR; +#if defined(__linux__) || defined(__PX4_LINUX) + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + + uart_config.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | ECHONL | ICANON | IEXTEN | ISIG); - uart_config.c_oflag &= !(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY); - uart_config.c_oflag |= NL0 | VT0; + // never send SIGTTOU + uart_config.c_lflag &= ~(TOSTOP); - uart_config.c_cflag &= !(CSIZE | CSTOPB | PARENB); - uart_config.c_cflag |= CS8 | CREAD | CLOCAL; + // ignore modem control lines + uart_config.c_cflag |= CLOCAL; - uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN); + // 8 bits + uart_config.c_cflag |= CS8; +#else /* __linux__ */ + + // Clear ONLCR flag (which appends a CR for every LF) + uart_config.c_oflag &= ~ONLCR; +#endif // Flow control - if (hw_flow_control) { + if (_hw_flow_control) { // HW flow control - uart_config.c_lflag |= CRTSCTS; - } else if (sw_flow_control) { + uart_config.c_cflag |= CRTSCTS; + uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); + } else if (_sw_flow_control) { // SW flow control + uart_config.c_cflag &= ~CRTSCTS; uart_config.c_lflag |= (IXON | IXOFF | IXANY); + } else { + uart_config.c_cflag &= ~CRTSCTS; + uart_config.c_iflag &= ~(IXON | IXOFF | IXANY); } // Set baud rate speed_t speed; - if (!baudrate_to_speed(baudrate, &speed)) { + if (!baudrate_to_speed(_baudrate, &speed)) { #ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n", - uart_name, baudrate); + printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: Unsupported _baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n", + _uart_name, _baudrate); #else - PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n", - uart_name, baudrate); + PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %" PRIu32 "\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n", + _uart_name, _baudrate); #endif /* PX4_ERR */ close(); return -EINVAL; @@ -349,77 +404,113 @@ int UART_node::init() if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", uart_name, termios_state, errno); + printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", _uart_name, termios_state, + errno); #else - PX4_ERR("ERR SET BAUD %s: %d (%d)", uart_name, termios_state, errno); + PX4_ERR("ERR SET BAUD %s: %d (%d)", _uart_name, termios_state, errno); #endif /* PX4_ERR */ close(); return -errno_bkp; } - if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { int errno_bkp = errno; #ifndef PX4_ERR - printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", uart_name, errno); + printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", _uart_name, errno); #else - PX4_ERR("UART transport: ERR SET CONF %s (%d)", uart_name, errno); + PX4_ERR("UART transport: ERR SET CONF %s (%d)", _uart_name, errno); #endif /* PX4_ERR */ close(); return -errno_bkp; } +#if defined(__linux__) || defined(__PX4_LINUX) + // For Linux, set high speed polling at the chip level. Since this routine relies on a USB latency + // change at the chip level it may fail on certain chip sets if their driver does not support this + // configuration request + { + struct serial_struct serial_ctl; + + if (ioctl(_uart_fd, TIOCGSERIAL, &serial_ctl) < 0) { + printf("\033[0;31m[ micrortps_transport ]\tError while trying to read serial port configuration: %d\033[0m\n", errno); + + if (ioctl(_uart_fd, TCFLSH, TCIOFLUSH) == -1) { + int errno_bkp = errno; + printf("\033[0;31m[ protocol__splitter ]\tCould not flush terminal\033[0m\n"); + close(); + return -errno_bkp; + } + } + + serial_ctl.flags |= ASYNC_LOW_LATENCY; + + if (ioctl(_uart_fd, TIOCSSERIAL, &serial_ctl) < 0) { + int errno_bkp = errno; + printf("\033[0;31m[ micrortps_transport ]\tError while trying to write serial port latency: %d\033[0m\n", errno); + close(); + return -errno_bkp; + } + } +#endif /* __linux__ */ + char aux[64]; bool flush = false; - while (0 < ::read(uart_fd, (void *)&aux, 64)) { + while (0 < ::read(_uart_fd, (void *)&aux, 64)) { flush = true; -/** - * According to px4_time.h, px4_usleep() is only defined when lockstep is set - * to be used - */ #ifndef px4_usleep usleep(1000); #else + /* With PX4 px4_usleep() should be used. */ px4_usleep(1000); #endif /* px4_usleep */ } if (flush) { #ifndef PX4_DEBUG - if (debug) printf("[ micrortps_transport ]\tUART transport: Flush\n"); + + if (_debug) { printf("[ micrortps_transport ]\tUART transport: Flush\n"); } + #else - if (debug) PX4_DEBUG("UART transport: Flush"); + + if (_debug) { PX4_DEBUG("UART transport: Flush"); } + #endif /* PX4_DEBUG */ + } else { #ifndef PX4_DEBUG - if (debug) printf("[ micrortps_transport ]\tUART transport: No flush\n"); + + if (_debug) { printf("[ micrortps_transport ]\tUART transport: No flush\n"); } + #else - if (debug) PX4_DEBUG("UART transport: No flush"); + + if (_debug) { PX4_DEBUG("UART transport: No flush"); } + #endif /* PX4_INFO */ } - poll_fd[0].fd = uart_fd; - poll_fd[0].events = POLLIN; + _poll_fd[0].fd = _uart_fd; + _poll_fd[0].events = POLLIN; - return uart_fd; + return _uart_fd; } bool UART_node::fds_OK() { - return (-1 != uart_fd); + return (-1 != _uart_fd); } uint8_t UART_node::close() { - if (-1 != uart_fd) { + if (-1 != _uart_fd) { #ifndef PX4_WARN printf("\033[1;33m[ micrortps_transport ]\tClosed UART.\n\033[0m"); #else PX4_WARN("Closed UART."); #endif /* PX4_WARN */ - ::close(uart_fd); - uart_fd = -1; - memset(&poll_fd, 0, sizeof(poll_fd)); + ::close(_uart_fd); + _uart_fd = -1; + memset(&_poll_fd, 0, sizeof(_poll_fd)); } return 0; @@ -432,10 +523,10 @@ ssize_t UART_node::node_read(void *buffer, size_t len) } ssize_t ret = 0; - int r = poll(poll_fd, 1, poll_ms); + int r = poll(_poll_fd, 1, _poll_ms); - if (r == 1 && (poll_fd[0].revents & POLLIN)) { - ret = ::read(uart_fd, buffer, len); + if (r == 1 && (_poll_fd[0].revents & POLLIN)) { + ret = ::read(_uart_fd, buffer, len); } return ret; @@ -447,7 +538,7 @@ ssize_t UART_node::node_write(void *buffer, size_t len) return -1; } - return ::write(uart_fd, buffer, len); + return ::write(_uart_fd, buffer, len); } bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) @@ -528,20 +619,17 @@ bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) case 2000000: *speed = B2000000; break; #ifdef B3000000 + case 3000000: *speed = B3000000; break; - case 3000000: *speed = B3000000; break; #endif - #ifdef B3500000 + case 3500000: *speed = B3500000; break; - case 3500000: *speed = B3500000; break; #endif - #ifdef B4000000 + case 4000000: *speed = B4000000; break; - case 4000000: *speed = B4000000; break; #endif - default: return false; } @@ -549,17 +637,17 @@ bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed) return true; } -UDP_node::UDP_node(const char* _udp_ip, uint16_t _udp_port_recv, - uint16_t _udp_port_send, const bool _debug): - Transport_node(_debug), - sender_fd(-1), - receiver_fd(-1), - udp_port_recv(_udp_port_recv), - udp_port_send(_udp_port_send) +UDP_node::UDP_node(const char *udp_ip, uint16_t udp_port_recv, + uint16_t udp_port_send, const uint8_t sys_id, const bool debug): + Transport_node(sys_id, debug), + _sender_fd(-1), + _receiver_fd(-1), + _udp_port_recv(udp_port_recv), + _udp_port_send(udp_port_send) { - if (nullptr != _udp_ip) { - strcpy(udp_ip, _udp_ip); - } + if (nullptr != udp_ip) { + strcpy(_udp_ip, udp_ip); + } } UDP_node::~UDP_node() @@ -569,7 +657,7 @@ UDP_node::~UDP_node() int UDP_node::init() { - if (0 > init_receiver(udp_port_recv) || 0 > init_sender(udp_port_send)) { + if (0 > init_receiver(_udp_port_recv) || 0 > init_sender(_udp_port_send)) { return -1; } @@ -578,19 +666,19 @@ int UDP_node::init() bool UDP_node::fds_OK() { - return (-1 != sender_fd && -1 != receiver_fd); + return (-1 != _sender_fd && -1 != _receiver_fd); } int UDP_node::init_receiver(uint16_t udp_port) { #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) // udp socket data - memset((char *)&receiver_inaddr, 0, sizeof(receiver_inaddr)); - receiver_inaddr.sin_family = AF_INET; - receiver_inaddr.sin_port = htons(udp_port); - receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); + memset((char *)&_receiver_inaddr, 0, sizeof(_receiver_inaddr)); + _receiver_inaddr.sin_family = AF_INET; + _receiver_inaddr.sin_port = htons(udp_port); + _receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY); - if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + if ((_receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { #ifndef PX4_ERR printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); #else @@ -598,13 +686,14 @@ int UDP_node::init_receiver(uint16_t udp_port) #endif /* PX4_ERR */ return -1; } + #ifndef PX4_INFO - printf("[ micrortps_transport ]\tUDP transport: Trying to connect..."); + printf("[ micrortps_transport ]\tUDP transport: Trying to connect...\n"); #else PX4_INFO("UDP transport: Trying to connect..."); #endif /* PX4_INFO */ - if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0) { + if (bind(_receiver_fd, (struct sockaddr *)&_receiver_inaddr, sizeof(_receiver_inaddr)) < 0) { #ifndef PX4_ERR printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Bind failed\033[0m\n"); #else @@ -612,6 +701,7 @@ int UDP_node::init_receiver(uint16_t udp_port) #endif /* PX4_ERR */ return -1; } + #ifndef PX4_INFO printf("[ micrortps_transport ]\tUDP transport: Connected to server!\n\n"); #else @@ -625,7 +715,7 @@ int UDP_node::init_sender(uint16_t udp_port) { #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + if ((_sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { #ifndef PX4_ERR printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n"); #else @@ -634,11 +724,11 @@ int UDP_node::init_sender(uint16_t udp_port) return -1; } - memset((char *) &sender_outaddr, 0, sizeof(sender_outaddr)); - sender_outaddr.sin_family = AF_INET; - sender_outaddr.sin_port = htons(udp_port); + memset((char *) &_sender_outaddr, 0, sizeof(_sender_outaddr)); + _sender_outaddr.sin_family = AF_INET; + _sender_outaddr.sin_port = htons(udp_port); - if (inet_aton(udp_ip, &sender_outaddr.sin_addr) == 0) { + if (inet_aton(_udp_ip, &_sender_outaddr.sin_addr) == 0) { #ifndef PX4_ERR printf("\033[0;31m[ micrortps_transport ]\tUDP transport: inet_aton() failed\033[0m\n"); #else @@ -656,26 +746,26 @@ uint8_t UDP_node::close() { #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - if (sender_fd != -1) { + if (_sender_fd != -1) { #ifndef PX4_WARN printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed sender socket!\033[0m\n"); #else PX4_WARN("UDP transport: Closed sender socket!"); #endif /* PX4_WARN */ - shutdown(sender_fd, SHUT_RDWR); - ::close(sender_fd); - sender_fd = -1; + shutdown(_sender_fd, SHUT_RDWR); + ::close(_sender_fd); + _sender_fd = -1; } - if (receiver_fd != -1) { + if (_receiver_fd != -1) { #ifndef PX4_WARN printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed receiver socket!\033[0m\n"); #else PX4_WARN("UDP transport: Closed receiver socket!"); #endif /* PX4_WARN */ - shutdown(receiver_fd, SHUT_RDWR); - ::close(receiver_fd); - receiver_fd = -1; + shutdown(_receiver_fd, SHUT_RDWR); + ::close(_receiver_fd); + _receiver_fd = -1; } #endif /* __PX4_NUTTX */ @@ -688,12 +778,12 @@ ssize_t UDP_node::node_read(void *buffer, size_t len) return -1; } - int ret = 0; + ssize_t ret = 0; #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) // Blocking call - static socklen_t addrlen = sizeof(receiver_outaddr); - ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr *) &receiver_outaddr, &addrlen); -#endif /* __PX4_NUTTX */ + static socklen_t addrlen = sizeof(_receiver_outaddr); + ret = recvfrom(_receiver_fd, buffer, len, 0, (struct sockaddr *)&_receiver_outaddr, &addrlen); +#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ return ret; } @@ -703,9 +793,9 @@ ssize_t UDP_node::node_write(void *buffer, size_t len) return -1; } - int ret = 0; + ssize_t ret = 0; #if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) - ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr)); -#endif /* __PX4_NUTTX */ + ret = sendto(_sender_fd, buffer, len, 0, (struct sockaddr *)&_sender_outaddr, sizeof(_sender_outaddr)); +#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */ return ret; } diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h index 784221050dc4..3605b63f8c95 100644 --- a/msg/templates/urtps/microRTPS_transport.h +++ b/msg/templates/urtps/microRTPS_transport.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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: @@ -41,19 +41,26 @@ #define BUFFER_SIZE 1024 #define DEFAULT_UART "/dev/ttyACM0" +namespace MicroRtps { + enum class System { + FMU, + MISSION_COMPUTER + }; +} + class Transport_node { public: - Transport_node(const bool _debug); + Transport_node(const uint8_t sys_id, const bool debug); virtual ~Transport_node(); virtual int init() {return 0;} virtual uint8_t close() {return 0;} - ssize_t read(uint8_t *topic_ID, char out_buffer[], size_t buffer_len); + ssize_t read(uint8_t *topic_id, char out_buffer[], size_t buffer_len); /** * write a buffer - * @param topic_ID + * @param topic_id * @param buffer buffer to write: it must leave get_header_length() bytes free at the beginning. This will be * filled with the header. length does not include get_header_length(). So buffer looks like this: * ------------------------------------------------- @@ -63,42 +70,46 @@ class Transport_node * @param length buffer length excluding header length * @return length on success, <0 on error */ - ssize_t write(const uint8_t topic_ID, char buffer[], size_t length); + ssize_t write(const uint8_t topic_id, char buffer[], size_t length); /** Get the Length of struct Header to make headroom for the size of struct Header along with payload */ size_t get_header_length(); -protected: - virtual ssize_t node_read(void *buffer, size_t len) = 0; - virtual ssize_t node_write(void *buffer, size_t len) = 0; - virtual bool fds_OK() = 0; - uint16_t crc16_byte(uint16_t crc, const uint8_t data); - uint16_t crc16(uint8_t const *buffer, size_t len); - -protected: - uint32_t rx_buff_pos; - char rx_buffer[BUFFER_SIZE] = {}; - bool debug = false; - uint8_t _seq_number{0}; - private: struct __attribute__((packed)) Header { char marker[3]; - uint8_t topic_ID; + uint8_t topic_id; + uint8_t sys_id; uint8_t seq; uint8_t payload_len_h; uint8_t payload_len_l; uint8_t crc_h; uint8_t crc_l; }; + +protected: + virtual ssize_t node_read(void *buffer, size_t len) = 0; + virtual ssize_t node_write(void *buffer, size_t len) = 0; + virtual bool fds_OK() = 0; + uint16_t crc16_byte(uint16_t crc, const uint8_t data); + uint16_t crc16(uint8_t const *buffer, size_t len); + + uint32_t _rx_buff_pos; + char _rx_buffer[BUFFER_SIZE]{}; + + bool _debug; + + uint8_t _sys_id; + uint8_t _seq_number{0}; }; class UART_node: public Transport_node { public: - UART_node(const char *_uart_name, const uint32_t _baudrate, - const uint32_t _poll_ms, const bool _hw_flow_control, - const bool _sw_flow_control, const bool _debug); + UART_node(const char *uart_name, const uint32_t baudrate, + const uint32_t poll_ms, const bool hw_flow_control, + const bool sw_flow_control, const uint8_t sys_id, + const bool debug); virtual ~UART_node(); int init(); @@ -110,20 +121,20 @@ class UART_node: public Transport_node bool fds_OK(); bool baudrate_to_speed(uint32_t bauds, speed_t *speed); - int uart_fd; - char uart_name[64] = {}; - uint32_t baudrate; - uint32_t poll_ms; - bool hw_flow_control = false; - bool sw_flow_control = false; - struct pollfd poll_fd[1] = {}; + int _uart_fd; + char _uart_name[64]{}; + uint32_t _baudrate; + uint32_t _poll_ms; + bool _hw_flow_control{false}; + bool _sw_flow_control{false}; + struct pollfd _poll_fd[1]{}; }; class UDP_node: public Transport_node { public: - UDP_node(const char* _udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send, - const bool _debug); + UDP_node(const char *udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send, + const uint8_t sys_id, const bool debug); virtual ~UDP_node(); int init(); @@ -136,12 +147,12 @@ class UDP_node: public Transport_node ssize_t node_write(void *buffer, size_t len); bool fds_OK(); - int sender_fd; - int receiver_fd; - char udp_ip[16] = {}; - uint16_t udp_port_recv; - uint16_t udp_port_send; - struct sockaddr_in sender_outaddr; - struct sockaddr_in receiver_inaddr; - struct sockaddr_in receiver_outaddr; + int _sender_fd; + int _receiver_fd; + char _udp_ip[16]{}; + uint16_t _udp_port_recv; + uint16_t _udp_port_send; + struct sockaddr_in _sender_outaddr; + struct sockaddr_in _receiver_inaddr; + struct sockaddr_in _receiver_outaddr; }; diff --git a/msg/templates/urtps/msg.idl.em b/msg/templates/urtps/msg.idl.em index a5b6d9ac91a0..1fa20e3fe64e 100644 --- a/msg/templates/urtps/msg.idl.em +++ b/msg/templates/urtps/msg.idl.em @@ -4,10 +4,15 @@ @# @# EmPy template for generating .idl files @# +@############################################### +@# Start of Template +@# +@# Context: +@# - spec (msggen.MsgSpec) Parsed specification of the .msg file @################################################################################ @# @# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -@# Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. +@# Copyright (c) 2018-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: @@ -37,9 +42,8 @@ @# @################################################################################ @{ -from packaging import version import genmsg.msgs - +from packaging import version from px_generate_uorb_topic_helper import * # this is in Tools/ builtin_types = set() diff --git a/msg/timesync.msg b/msg/timesync.msg index 45c92f27e7b6..67f415c74451 100644 --- a/msg/timesync.msg +++ b/msg/timesync.msg @@ -1,5 +1,4 @@ uint64 timestamp # time since system start (microseconds) -uint8 sys_id # id of the origin system uint8 seq # timesync msg sequence int64 tc1 # time sync timestamp 1 int64 ts1 # time sync timestamp 2 diff --git a/msg/timesync_status.msg b/msg/timesync_status.msg index 38ff31db7542..9d557352dc99 100644 --- a/msg/timesync_status.msg +++ b/msg/timesync_status.msg @@ -1,5 +1,10 @@ -uint64 timestamp # time since system start (microseconds) -uint64 remote_timestamp # remote system timestamp (microseconds) -int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) -int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) -uint32 round_trip_time # round trip time of this timesync packet (microseconds) +uint64 timestamp # time since system start (microseconds) + +uint8 SOURCE_PROTOCOL_MAVLINK = 0 +uint8 SOURCE_PROTOCOL_RTPS = 1 +uint8 source_protocol # timesync source. Source can be MAVLink or the microRTPS bridge + +uint64 remote_timestamp # remote system timestamp (microseconds) +int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) +int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) +uint32 round_trip_time # round trip time of this timesync packet (microseconds) diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py index 3314b9caa567..0ff2de648221 100644 --- a/msg/tools/generate_microRTPS_bridge.py +++ b/msg/tools/generate_microRTPS_bridge.py @@ -2,7 +2,7 @@ ################################################################################ # # Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima). -# Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2018-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: @@ -42,7 +42,6 @@ import argparse import shutil import px_generate_uorb_topic_files -import px_generate_uorb_topic_helper from uorb_rtps_classifier import Classifier import subprocess import glob @@ -70,81 +69,11 @@ sys.exit(1) -def check_rtps_id_uniqueness(classifier): - """ - Checks if there are no ID's for different msgs repeated on the map - """ - - repeated_ids = dict() - - full_send_list = dict(list(msg for msg in list(classifier.msgs_to_send.items( - ))) + list(list(msg[0].items())[0] for msg in classifier.alias_msgs_to_send)) - full_receive_list = dict(list(msg for msg in list(classifier.msgs_to_receive.items( - ))) + list(list(msg[0].items())[0] for msg in classifier.alias_msgs_to_receive)) - full_ignore_list = dict(list(msg for msg in list(classifier.msgs_to_ignore.items( - ))) + list(list(msg[0].items())[0] for msg in classifier.alias_msgs_to_ignore)) - - # check if there are repeated ID's on the messages to send - for key, value in list(full_send_list.items()): - if list(full_send_list.values()).count(value) > 1: - repeated_ids.update({key: value}) - - # check if there are repeated ID's on the messages to receive - for key, value in list(full_receive_list.items()): - if list(full_receive_list.values()).count(value) > 1: - repeated_ids.update({key: value}) - - # check if there are repeated ID's on the messages to ignore - for key, value in list(full_ignore_list.items()): - if list(full_ignore_list.values()).count(value) > 1: - repeated_ids.update({key: value}) - - # check if there are repeated IDs between classified and unclassified msgs - # check send and ignore lists - send_ignore_common_ids = list(set(full_ignore_list.values( - )).intersection(list(full_send_list.values()))) - for item in list(full_send_list.items()): - for repeated in send_ignore_common_ids: - if item[1] == repeated: - repeated_ids.update({item[0]: item[1]}) - for item in list(full_ignore_list.items()): - for repeated in send_ignore_common_ids: - if item[1] == repeated: - repeated_ids.update({item[0]: item[1]}) - - # check receive and ignore lists - receive_ignore_common_ids = list(set(full_ignore_list.values( - )).intersection(list(full_receive_list.values()))) - for item in list(full_receive_list.items()): - for repeated in receive_ignore_common_ids: - if item[1] == repeated: - repeated_ids.update({item[0]: item[1]}) - for item in list(full_ignore_list.items()): - for repeated in receive_ignore_common_ids: - if item[1] == repeated: - repeated_ids.update({item[0]: item[1]}) - - all_msgs = {} - all_msgs.update(full_send_list) - all_msgs.update(full_receive_list) - all_msgs.update(full_ignore_list) - all_ids = list() - all_ids = list(all_msgs.values()) - all_ids.sort() - - if not repeated_ids: - print("All good. RTPS ID's are unique") - else: - raise AssertionError(", ".join('%s' % msgs for msgs in list(repeated_ids.keys())) + - " have their ID's repeated. Please choose from the following pool:\n" + - ", ".join('%d' % id for id in px_generate_uorb_topic_helper.check_available_ids(all_ids))) - - default_client_out = "src/modules/micrortps_bridge/micrortps_client" default_agent_out = "src/modules/micrortps_bridge/micrortps_agent" default_uorb_templates_dir = "templates/uorb_microcdr" default_urtps_templates_dir = "templates/urtps" -default_rtps_id_file = "tools/uorb_rtps_message_ids.yaml" +default_urtps_topics_file = "tools/urtps_bridge_topics.yaml" default_package_name = px_generate_uorb_topic_files.PACKAGE parser = argparse.ArgumentParser() @@ -168,7 +97,7 @@ def check_rtps_id_uniqueness(classifier): parser.add_argument("-q", "--urtps-templates-dir", dest='urtps_templates', type=str, help="uRTPS templates, by default using relative path to msgdir 'templates/urtps'", default=default_urtps_templates_dir) parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="RTPS msg IDs definition path, by default using relative path to msgdir 'tools/uorb_rtps_message_ids.yaml'", default=default_rtps_id_file) + help="Setup uRTPS bridge topics file path, by default using relative path to msgdir 'tools/urtps_bridge_topics.yaml'", default=default_urtps_topics_file) parser.add_argument("-p", "--package", dest='package', type=str, help="Msg package naming, by default px4", default=default_package_name) parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, @@ -319,13 +248,10 @@ def check_rtps_id_uniqueness(classifier): urtps_templates_dir = (args.urtps_templates if os.path.isabs(args.urtps_templates) else os.path.join(msg_dir, args.urtps_templates)) -# parse yaml file into a map of ids +# parse yaml file into a map of ids and messages to send and receive classifier = (Classifier(os.path.abspath(args.yaml_file), msg_dir) if os.path.isabs(args.yaml_file) else Classifier(os.path.join(msg_dir, args.yaml_file), msg_dir)) -# check if there are no ID's repeated -check_rtps_id_uniqueness(classifier) - uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.em' uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.em' @@ -348,74 +274,74 @@ def generate_agent(out_dir): if gen_idl: if out_dir != agent_out_dir: px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) else: px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) if classifier.alias_msgs_to_send: for msg_file in classifier.alias_msgs_to_send: - msg_alias = list(msg_file[0].keys())[0] + msg_alias = msg_file[0] msg_name = msg_file[1] if gen_idl: if out_dir != agent_out_dir: px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) else: px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_SRC_TEMPL_FILE) px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_PUBLISHER_H_TEMPL_FILE) if classifier.msgs_to_receive: for msg_file in classifier.msgs_to_receive: if gen_idl: if out_dir != agent_out_dir: px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) else: px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) if classifier.alias_msgs_to_receive: for msg_file in classifier.alias_msgs_to_receive: - msg_alias = list(msg_file[0].keys())[0] + msg_alias = msg_file[0] msg_name = msg_file[1] if gen_idl: if out_dir != agent_out_dir: px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) else: px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_id_map) + package, px_generate_uorb_topic_files.INCL_DEFAULT, fastrtps_version, ros2_distro, classifier.msg_list) px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir, - package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) + package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_SUBSCRIBER_H_TEMPL_FILE) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_AGENT_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TEMPL_FILE) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_CPP_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_CPP_TEMPL_FILE) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_H_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_TIMESYNC_H_TEMPL_FILE) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, out_dir, - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) if cmakelists: px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, os.path.dirname(out_dir), - urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE) + urtps_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE) # Final steps to install agent mkdir_p(os.path.join(out_dir, "fastrtpsgen")) @@ -428,15 +354,21 @@ def generate_agent(out_dir): # the '-typeros2' option in fastrtpsgen. # .. note:: This is only available in FastRTPSGen 1.0.4 and above gen_ros2_typename = "" - if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy'] and fastrtpsgen_version >= version.Version("1.0.4"): + if ros2_distro and ros2_distro in ['dashing', 'eloquent', 'foxy', 'galactic', 'rolling'] and fastrtpsgen_version >= version.Version("1.0.4"): gen_ros2_typename = "-typeros2 " + idl_files = [] for idl_file in glob.glob(os.path.join(idl_dir, "*.idl")): - try: - ret = subprocess.check_call(fastrtpsgen_path + " -d " + out_dir + - "/fastrtpsgen -example x64Linux2.6gcc " + gen_ros2_typename + fastrtpsgen_include + idl_file, shell=True) - except OSError: - raise + # Only run the generator for the topics that are actually marked to be + # used by the bridge + if os.path.splitext(os.path.basename(idl_file))[0] in classifier.msg_list: + idl_files.append(idl_file) + + try: + ret = subprocess.check_call(fastrtpsgen_path + " -d " + out_dir + + "/fastrtpsgen -example x64Linux2.6gcc " + gen_ros2_typename + fastrtpsgen_include + " ".join(str(idl_file) for idl_file in idl_files), shell=True) + except OSError: + raise rm_wildcard(os.path.join(out_dir, "fastrtpsgen/*PubSubMain*")) rm_wildcard(os.path.join(out_dir, "fastrtpsgen/makefile*")) @@ -494,7 +426,17 @@ def generate_client(out_dir): os.rename(def_file, def_file.replace(".h", ".h_")) px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, - out_dir, uorb_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, fastrtps_version, ros2_distro, uRTPS_CLIENT_TEMPL_FILE) + out_dir, uorb_templates_dir, package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_list, fastrtps_version, ros2_distro, uRTPS_CLIENT_TEMPL_FILE) + + px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, + out_dir, + uorb_templates_dir, + package, + px_generate_uorb_topic_files.INCL_DEFAULT, + classifier.msg_list, + fastrtps_version, + ros2_distro, + 'dds_topics.h.em') # Final steps to install client cp_wildcard(os.path.join(urtps_templates_dir, diff --git a/msg/tools/generate_msg_docs.py b/msg/tools/generate_msg_docs.py new file mode 100755 index 000000000000..c3f542a8e6d3 --- /dev/null +++ b/msg/tools/generate_msg_docs.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 + +""" +Generate docs from .msg files +""" + +import os +import argparse +import sys + + +def get_msgs_list(msgdir): + """ + Makes list of msg files in the given directory + """ + return [fn for fn in os.listdir(msgdir) if fn.endswith(".msg")] + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Generate docs from .msg files') + parser.add_argument('-d', dest='dir', help='output directory', required=True) + args = parser.parse_args() + + output_dir = args.dir + if not os.path.isdir(output_dir): + os.mkdir(output_dir) + + msg_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"..") + msg_files = get_msgs_list(msg_path) + msg_files.sort() + + filelist_in_markdown='' + + for msg_file in msg_files: + msg_name = os.path.splitext(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)) + + #Format msg url + msg_url="[source file](https://github.com/PX4/PX4-Autopilot/blob/master/msg/%s)" % msg_file + + msg_description = "" + summary_description = "" + + #Get msg description (first non-empty comment line from top of msg) + with open(msg_filename, 'r') as lineparser: + line = lineparser.readline() + while line.startswith('#') or (line.strip() == ''): + print('DEBUG: line: %s' % line) + line=line[1:].strip()+'\n' + stripped_line=line.strip() + if msg_description and not summary_description and stripped_line=='': + summary_description = msg_description.strip() + + msg_description+=line + line = lineparser.readline() + msg_description=msg_description.strip() + if not summary_description and msg_description: + summary_description = msg_description + print('msg_description: Z%sZ' % msg_description) + print('summary_description: Z%sZ' % summary_description) + summary_description + msg_contents = "" + #Get msg contents (read the file) + with open(msg_filename, 'r') as source_file: + msg_contents = source_file.read() + + #Format markdown using msg name, comment, url, contents. + markdown_output="""# %s (UORB message) + +%s + +%s + +```c +%s +``` +""" % (msg_name, msg_description, msg_url, msg_contents) + + with open(output_file, 'w') as content_file: + content_file.write(markdown_output) + + readme_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) + if summary_description: + readme_markdown_file_link+=" — %s" % summary_description + filelist_in_markdown+=readme_markdown_file_link+"\n" + + # Write out the README.md file + readme_text="""# uORB Message Reference + +:::note +This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/master/msg/tools/generate_msg_docs.py) from the source code. +::: + +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)). +Graphs showing how these are used [can be found here](../middleware/uorb_graph.md). + +%s + """ % (filelist_in_markdown) + readme_file = os.path.join(output_dir, 'README.md') + with open(readme_file, 'w') as content_file: + content_file.write(readme_text) diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py index 553a7715721f..e7894bd92be0 100755 --- a/msg/tools/px_generate_uorb_topic_files.py +++ b/msg/tools/px_generate_uorb_topic_files.py @@ -98,9 +98,11 @@ class MsgScope: RECEIVE = 2 -def get_multi_topics(filename): +def get_topics(filename, msg_name): """ - Get TOPICS names from a "# TOPICS" line + Get TOPICS names from a "# TOPICS" line. If there are no multi topics defined, + set topic name same as the message name, since the user doesn't expect any new + custom topic names. """ ofile = open(filename, 'r') text = ofile.read() @@ -111,6 +113,10 @@ def get_multi_topics(filename): topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "") result.extend(topic_names_str.split(" ")) ofile.close() + + if len(result) == 0: + result.append(msg_name) + return result @@ -147,15 +153,16 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name + " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!") exit(1) - topics = get_multi_topics(filename) + + # Get topics used for the message + topics = get_topics(filename, spec.short_name) + if includepath: search_path = genmsg.command_line.includepath_to_dict(includepath) else: search_path = {} genmsg.msg_loader.load_depends(msg_context, spec, search_path) md5sum = genmsg.gentools.compute_md5(msg_context, spec) - if len(topics) == 0: - topics.append(spec.short_name) em_globals = { "file_name_in": filename, "md5sum": md5sum, @@ -177,7 +184,7 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template return generate_by_template(output_file, template_file, em_globals) -def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, fastrtps_version, ros2_distro, ids): +def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, fastrtps_version, ros2_distro, msgs): """ Generates an .idl from .msg file """ @@ -185,11 +192,11 @@ def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, pack if (alias != ""): em_globals = get_em_globals( - msg, alias, package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.NONE) + msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) spec_short_name = alias else: em_globals = get_em_globals( - msg, "", package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.NONE) + msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) spec_short_name = em_globals["spec"].short_name # Make sure output directory exists: @@ -208,7 +215,7 @@ def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, pack def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filename_receive_msgs, filename_alias_receive_msgs, - msg_dir, outputdir, templatedir, package, includepath, ids, fastrtps_version, ros2_distro, template_name): + msg_dir, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): """ Generates source file by msg content """ @@ -218,27 +225,27 @@ def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filenam for msg in filename_receive_msgs) alias_send_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), list(msg[0].keys())[0]] for msg in filename_alias_send_msgs) + msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_send_msgs) alias_receive_msgs = list([os.path.join( - msg_dir, msg[1] + ".msg"), list(msg[0].keys())[0]] for msg in filename_alias_receive_msgs) + msg_dir, msg[1] + ".msg"), msg[0]] for msg in filename_alias_receive_msgs) em_globals_list = [] if send_msgs: em_globals_list.extend([get_em_globals( - f, "", package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.SEND) for f in send_msgs]) + f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in send_msgs]) if alias_send_msgs: em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.SEND) for f in alias_send_msgs]) + f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.SEND) for f in alias_send_msgs]) if receive_msgs: em_globals_list.extend([get_em_globals( - f, "", package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in receive_msgs]) + f, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in receive_msgs]) if alias_receive_msgs: em_globals_list.extend([get_em_globals( - f[0], f[1], package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in alias_receive_msgs]) + f[0], f[1], package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.RECEIVE) for f in alias_receive_msgs]) merged_em_globals = merge_em_globals_list(em_globals_list) @@ -253,7 +260,7 @@ def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filenam return generate_by_template(output_file, template_file, merged_em_globals) -def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, ids, fastrtps_version, ros2_distro, template_name): +def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, msgs, fastrtps_version, ros2_distro, template_name): """ Generates a sources and headers from .msg file """ @@ -261,11 +268,11 @@ def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, pa if (alias): em_globals = get_em_globals( - msg, alias, package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.NONE) + msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) spec_short_name = alias else: em_globals = get_em_globals( - msg, "", package, includepath, ids, fastrtps_version, ros2_distro, MsgScope.NONE) + msg, "", package, includepath, msgs, fastrtps_version, ros2_distro, MsgScope.NONE) spec_short_name = em_globals["spec"].short_name # Make sure output directory exists: @@ -279,7 +286,7 @@ def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, pa return generate_by_template(output_file, template_file, em_globals) -def get_em_globals(filename_msg, alias, package, includepath, ids, fastrtps_version, ros2_distro, scope): +def get_em_globals(filename_msg, alias, package, includepath, msgs, fastrtps_version, ros2_distro, scope): """ Generates em globals dictionary """ @@ -288,15 +295,16 @@ def get_em_globals(filename_msg, alias, package, includepath, ids, fastrtps_vers package, os.path.basename(filename_msg)) spec = genmsg.msg_loader.load_msg_from_file( msg_context, filename_msg, full_type_name) - topics = get_multi_topics(filename_msg) + + # Get topics used for the message + topics = get_topics(filename_msg, spec.short_name) + if includepath: search_path = genmsg.command_line.includepath_to_dict(includepath) else: search_path = {} genmsg.msg_loader.load_depends(msg_context, spec, search_path) md5sum = genmsg.gentools.compute_md5(msg_context, spec) - if len(topics) == 0: - topics.append(spec.short_name) em_globals = { "file_name_in": filename_msg, "md5sum": md5sum, @@ -304,7 +312,7 @@ def get_em_globals(filename_msg, alias, package, includepath, ids, fastrtps_vers "msg_context": msg_context, "spec": spec, "topics": topics, - "ids": ids, + "msgs": msgs, "scope": scope, "package": package, "alias": alias, @@ -452,24 +460,31 @@ def convert_dir_save(format_idx, inputdir, outputdir, package, templatedir, temp def generate_topics_list_file(msgdir, outputdir, template_filename, templatedir): # generate cpp file with topics list msgs = get_msgs_list(msgdir) - multi_topics = [] + topics = [] for msg in msgs: msg_filename = os.path.join(msgdir, msg) - multi_topics.extend(get_multi_topics(msg_filename)) - tl_globals = {"msgs": msgs, "multi_topics": multi_topics} + topics.extend(get_topics(msg_filename, msg)) + tl_globals = {"msgs": msgs, "topics": topics} tl_template_file = os.path.join(templatedir, template_filename) tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) generate_by_template(tl_out_file, tl_template_file, tl_globals) def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): - # generate cpp file with topics list - filenames = [os.path.basename( - p) for p in files if os.path.basename(p).endswith(".msg")] - multi_topics = [] - for msg_filename in files: - multi_topics.extend(get_multi_topics(msg_filename)) - tl_globals = {"msgs": filenames, "multi_topics": multi_topics} + # Get message file names ending with .msg only + msg_filenames = [p for p in files if os.path.basename(p).endswith(".msg")] + + # Get topics used in messages + topics = [] + for msg_filename in msg_filenames: + msg_name = os.path.basename(msg_filename).replace('.msg', '') + topics.extend(get_topics(msg_filename, msg_name)) + + # Get only the message file name for "msgs" component + msg_basenames = [os.path.basename(p) for p in msg_filenames] + + # Set the Template dictionary settings + tl_globals = {"msgs": msg_basenames, "topics": topics} tl_template_file = os.path.join(templatedir, template_filename) tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) generate_by_template(tl_out_file, tl_template_file, tl_globals) @@ -529,7 +544,10 @@ def append_to_include_path(path_to_append, curr_include, package): generate_output_from_file( generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT) - generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir) + # Generate topics list header and source file + if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])): + generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir) + copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet) elif args.dir is not None: convert_dir_save( diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index fc8e4cf05caa..1f90a01ddf8d 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 ############################################################################# # -# Copyright (C) 2013-2019 PX4 Pro Development Team. All rights reserved. +# Copyright (C) 2013-2021 PX4 Pro Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -60,6 +60,23 @@ 'char': 'char', } +type_map_short = { + # We use some range outside of alpha-numeric and special characters. + # This needs to match with orb_get_c_type() + 'int8': '\\x82', + 'int16': '\\x83', + 'int32': '\\x84', + 'int64': '\\x85', + 'uint8': '\\x86', + 'uint16': '\\x87', + 'uint32': '\\x88', + 'uint64': '\\x89', + 'float32': '\\x8a', + 'float64': '\\x8b', + 'bool': '\\x8c', + 'char': '\\x8d', +} + type_serialize_map = { 'int8': 'int8_t', 'int16': 'int16_t', @@ -204,7 +221,7 @@ def add_padding_bytes(fields, search_path): return (struct_size, num_padding_bytes) -def convert_type(spec_type): +def convert_type(spec_type, use_short_type=False): """ Convert from msg type to C type """ @@ -215,128 +232,15 @@ def convert_type(spec_type): msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) c_type = msg_type - if msg_type in type_map: + if use_short_type and msg_type in type_map_short: + c_type = type_map_short[msg_type] + elif msg_type in type_map: c_type = type_map[msg_type] if is_array: return c_type + "[" + str(array_length) + "]" return c_type -def print_field(field): - """ - Echo printf line - """ - - # check if there are any upper case letters in the field name - assert not any(a.isupper() - for a in field.name), "%r field contains uppercase letters" % field.name - - # skip padding - if field.name.startswith('_padding'): - return - - bare_type = field.type - if '/' in field.type: - # removing prefix - bare_type = (bare_type.split('/'))[1] - - msg_type, is_array, array_length = genmsg.msgs.parse_type(bare_type) - - field_name = "" - - if is_array: - c_type = "[" - - if msg_type in type_map: - p_type = type_printf_map[msg_type] - - else: - for i in range(array_length): - print(("PX4_INFO_RAW(\"\\t" + field.type + - " " + field.name + "[" + str(i) + "]\");")) - print((" print_message(message." + - field.name + "[" + str(i) + "]);")) - return - - for i in range(array_length): - - if i > 0: - c_type += ", " - field_name += ", " - - if "float32" in field.type: - field_name += "(double)message." + \ - field.name + "[" + str(i) + "]" - else: - field_name += "message." + field.name + "[" + str(i) + "]" - - c_type += str(p_type) - - c_type += "]" - - else: - c_type = msg_type - if msg_type in type_map: - c_type = type_printf_map[msg_type] - - field_name = "message." + field.name - - # cast double - if field.type == "float32": - field_name = "(double)" + field_name - elif field.type == "bool": - c_type = '%s' - field_name = "(" + field_name + " ? \"True\" : \"False\")" - - else: - print(("PX4_INFO_RAW(\"\\n\\t" + field.name + "\");")) - print(("\tprint_message(message." + field.name + ");")) - return - - if field.name == 'timestamp': - print(("if (message.timestamp != 0) {\n\t\tPX4_INFO_RAW(\"\\t" + field.name + - ": " + c_type + " (%.6f seconds ago)\\n\", " + field_name + - ", (now - message.timestamp) / 1e6);\n\t} else {\n\t\tPX4_INFO_RAW(\"\\n\");\n\t}")) - elif field.name == 'timestamp_sample': - print(("\n\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (" + c_type + " us before timestamp)\\n\", " + field_name + ", message.timestamp - message.timestamp_sample);\n\t")) - elif field.name == 'device_id': - print("char device_id_buffer[80];") - print("device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), message.device_id);") - print("PX4_INFO_RAW(\"\\tdevice_id: %d (%s) \\n\", message.device_id, device_id_buffer);") - elif field.name == 'accel_device_id': - print("char accel_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(accel_device_id_buffer, sizeof(accel_device_id_buffer), message.accel_device_id);") - print("PX4_INFO_RAW(\"\\taccel_device_id: %d (%s) \\n\", message.accel_device_id, accel_device_id_buffer);") - elif field.name == 'gyro_device_id': - print("char gyro_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(gyro_device_id_buffer, sizeof(gyro_device_id_buffer), message.gyro_device_id);") - print("PX4_INFO_RAW(\"\\tgyro_device_id: %d (%s) \\n\", message.gyro_device_id, gyro_device_id_buffer);") - elif field.name == 'baro_device_id': - print("char baro_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(baro_device_id_buffer, sizeof(baro_device_id_buffer), message.baro_device_id);") - print("PX4_INFO_RAW(\"\\tbaro_device_id: %d (%s) \\n\", message.baro_device_id, baro_device_id_buffer);") - elif field.name == 'mag_device_id': - print("char mag_device_id_buffer[80];") - print("device::Device::device_id_print_buffer(mag_device_id_buffer, sizeof(mag_device_id_buffer), message.mag_device_id);") - print("PX4_INFO_RAW(\"\\tmag_device_id: %d (%s) \\n\", message.mag_device_id, mag_device_id_buffer);") - elif (field.name == 'q' or 'q_' in field.name) and field.type == 'float32[4]': - # float32[4] q/q_d/q_reset/delta_q_reset - print("{") - print("\t\tmatrix::Eulerf euler{matrix::Quatf{message." + field.name + "}};") - print("\t\tPX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg" ")\\n\", " + field_name + ", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));\n\t") - print("\t}") - - elif ("flags" in field.name or "bits" in field.name) and "uint" in field.type: - # print bits of fixed width unsigned integers (uint8, uint16, uint32) if name contains flags or bits - print("PX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + " (0b\", " + field_name + ");") - print("\tfor (int i = (sizeof(" + field_name + ") * 8) - 1; i >= 0; i--) { PX4_INFO_RAW(\"%u%s\", " + field_name + " >> i & 1, ((unsigned)i < (sizeof(" + field_name + ") * 8) - 1 && i % 4 == 0 && i > 0) ? \"'\" : \"\"); }") - print("\tPX4_INFO_RAW(\")\\n\");") - elif is_array and 'char' in field.type: - print(("PX4_INFO_RAW(\"\\t" + field.name + ": \\\"%." + str(array_length) + "s\\\" \\n\", message." + field.name + ");")) - else: - print(("PX4_INFO_RAW(\"\\t" + field.name + ": " + c_type + "\\n\", " + field_name + ");")) - - def print_field_def(field): """ Print the C type from a field @@ -375,37 +279,4 @@ def print_field_def(field): comment = ' // required for logger' print(('\t%s%s%s %s%s;%s' % (type_prefix, type_px4, type_appendix, field.name, - array_size, comment))) - - -def check_available_ids(used_msg_ids_list): - """ - Checks the available RTPS ID's - """ - return set(list(range(0, 255))) - set(used_msg_ids_list) - - -def rtps_message_id(msg_id_map, message): - """ - Get RTPS ID of uORB message - """ - error_msg = "" - - # check if the message has an ID set - for dict in msg_id_map[0]['rtps']: - if message in dict['msg']: - if dict['id'] is not None: - return dict['id'] - else: - error_msg = "ID is None!" - break - - # create list of the available IDs if it fails to get an ID - used_ids = list() - for dict in msg_id_map[0]['rtps']: - if dict['id'] is not None: - used_ids.append(dict['id']) - - raise AssertionError( - "%s %s Please add an ID from the available pool:\n" % (message, error_msg) + - ", ".join('%d' % id for id in check_available_ids(used_ids))) + array_size, comment))) diff --git a/msg/tools/uorb_rtps_classifier.py b/msg/tools/uorb_rtps_classifier.py index c09b8567cc5e..13a79ffa329b 100644 --- a/msg/tools/uorb_rtps_classifier.py +++ b/msg/tools/uorb_rtps_classifier.py @@ -31,143 +31,79 @@ # ################################################################################ -import sys -import os import argparse +import difflib import errno +import os +from typing import Dict, List, Tuple import yaml -import re -import difflib class Classifier(): - """ - Class to classify RTPS msgs as senders, receivers or to be ignored - """ + """Class to classify RTPS msgs as to send, receive and set their IDs.""" - def __init__(self, yaml_file, msg_folder): + def __init__(self, yaml_file, msg_folder) -> None: self.msg_folder = msg_folder - self.all_msgs_list = self.set_all_msgs() - self.msg_id_map = self.parse_yaml_msg_id_file(yaml_file) - self.alias_space_init_id = 170 + self.msg_map = self.parse_yaml_msgs_file(yaml_file) - # Checkers - self.check_if_listed(yaml_file) + # Check if base types are defined correctly self.check_base_type() - self.check_id_space() - self.msgs_to_send, self.alias_msgs_to_send = self.set_msgs_to_send() - self.msgs_to_receive, self.alias_msgs_to_receive = self.set_msgs_to_receive() - self.msgs_to_ignore, self.alias_msgs_to_ignore = self.set_msgs_to_ignore() - self.msg_files_send = self.set_msg_files_send() - self.msg_files_receive = self.set_msg_files_receive() - self.msg_files_ignore = self.set_msg_files_ignore() + # Get messages to send and to receive + self.msgs_to_send: Dict[str, int] = dict() + self.msgs_to_receive: Dict[str, int] = dict() + self.alias_msgs_to_send: List[Tuple[str, str]] = [] + self.alias_msgs_to_receive: List[Tuple[str, str]] = [] + self.msg_list: List[str] = [] - # setters (for class init) - def set_all_msgs(self): - msg_list = [] - for filename in os.listdir(self.msg_folder): - if '.msg' in filename: - # add base messages - msg_list.append(re.sub(".msg", "", filename)) + # Create message map + self.setup_msg_map() - # add alias / multi-topics messages - with open(os.path.join(self.msg_folder, filename), 'r') as f: - alias_msgs = [] - lines = f.readlines() - for line in lines: - if '# TOPICS' in line: - fileUpdated = True - alias_msgs += line.split() - alias_msgs.remove('#') - alias_msgs.remove('TOPICS') - for msg in alias_msgs: - if msg not in msg_list: - msg_list.append(msg) - return msg_list - - def set_msgs_to_send(self): - send = {} - send_alias = [] - for dict in self.msg_id_map['rtps']: - if 'send' in list(dict.keys()): - if 'alias' in list(dict.keys()): - send_alias.append( - ({dict['msg']: dict['id']}, dict['alias'])) - else: - send.update({dict['msg']: dict['id']}) - return send, send_alias + self.msg_files_send = self.set_msg_files_send() + self.msg_files_receive = self.set_msg_files_receive() - def set_msgs_to_receive(self): - receive = {} - receive_alias = [] - for dict in self.msg_id_map['rtps']: - if 'receive' in list(dict.keys()): - if 'alias' in list(dict.keys()): - receive_alias.append( - ({dict['msg']: dict['id']}, dict['alias'])) + def setup_msg_map(self) -> None: + """Setup dictionary with an ID map for the messages.""" + for topic in self.msg_map['rtps']: + if 'send' in list(topic.keys()): + if 'base' in list(topic.keys()): + self.alias_msgs_to_send.append( + (topic['msg'], topic['base'])) else: - receive.update({dict['msg']: dict['id']}) - return receive, receive_alias - - def set_msgs_to_ignore(self): - ignore = {} - ignore_alias = [] - for dict in self.msg_id_map['rtps']: - if (('send' not in list(dict.keys())) and ('receive' not in list(dict.keys()))): - if 'alias' in list(dict.keys()): - ignore_alias.append( - ({dict['msg']: dict['id']}, dict['alias'])) + self.msgs_to_send.update({topic['msg']: 0}) + if 'receive' in list(topic.keys()): + if 'base' in list(topic.keys()): + self.alias_msgs_to_receive.append( + (topic['msg'], topic['base'])) else: - ignore.update({dict['msg']: dict['id']}) - return ignore, ignore_alias + self.msgs_to_receive.update({topic['msg']: 0}) + self.msg_list.append(topic['msg']) - def set_msg_files_send(self): + def set_msg_files_send(self) -> list: + """ + Append the path to the files which messages are marked to + be sent. + """ return [os.path.join(self.msg_folder, msg + ".msg") for msg in list(self.msgs_to_send.keys())] - def set_msg_files_receive(self): - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_receive.keys())] - - def set_msg_files_ignore(self): - return [os.path.join(self.msg_folder, msg + ".msg") - for msg in list(self.msgs_to_ignore.keys())] - - def check_if_listed(self, yaml_file): + def set_msg_files_receive(self) -> list: """ - Checks if all msgs are listed under the RTPS ID yaml file + Append the path to the files which messages are marked to + be received. """ - none_listed_msgs = [] - for msg in self.all_msgs_list: - result = not any( - dict['msg'] == msg for dict in self.msg_id_map['rtps']) - if result: - none_listed_msgs.append(msg) - - if len(none_listed_msgs) > 0: - if len(none_listed_msgs) == 1: - error_msg = "The following message is not listen under " - elif len(none_listed_msgs) > 1: - error_msg = "The following messages are not listen under " - - raise AssertionError( - "\n%s %s: " % (error_msg, yaml_file) + ", ".join('%s' % msg for msg in none_listed_msgs) + - "\n\nPlease add them to the yaml file with the respective ID and, if applicable, mark them " + - "to be sent or received by the micro-RTPS bridge.\n" - "NOTE: If the message is an alias / part of multi-topics (#TOPICS) of a base message, it should be added as well.\n") + return [os.path.join(self.msg_folder, msg + ".msg") + for msg in list(self.msgs_to_receive.keys())] - def check_base_type(self): - """ - Check if alias message has correct base type - """ + def check_base_type(self) -> None: + """Check if alias message has correct base type.""" registered_alias_msgs = list( - dict['alias'] for dict in self.msg_id_map['rtps'] if 'alias' in list(dict.keys())) + topic['base'] for topic in self.msg_map['rtps'] if 'base' in list(topic.keys())) base_types = [] - for dict in self.msg_id_map['rtps']: - if 'alias' not in list(dict.keys()): - base_types.append(dict['msg']) + for topic in self.msg_map['rtps']: + if 'base' not in list(topic.keys()): + base_types.append(topic['msg']) incorrect_base_types = list( set(registered_alias_msgs) - set(base_types)) @@ -181,40 +117,17 @@ def check_base_type(self): raise AssertionError( ('\n' + '\n'.join('\t- The multi-topic message base type \'{}\' does not exist.{}'.format(k, (' Did you mean \'' + v[0] + '\'?' if v else '')) for k, v in list(base_types_suggestion.items())))) - def check_id_space(self): - """ - Check if msg ID is in the correct ID space - """ - incorrect_base_ids = {} - incorrect_alias_ids = {} - for dict in self.msg_id_map['rtps']: - if 'alias' not in list(dict.keys()) and dict['id'] >= self.alias_space_init_id: - incorrect_base_ids.update({dict['msg']: dict['id']}) - elif 'alias' in list(dict.keys()) and dict['id'] < self.alias_space_init_id: - incorrect_alias_ids.update({dict['msg']: dict['id']}) - - if len(incorrect_base_ids) > 0: - raise AssertionError( - ('\n' + '\n'.join('\t- The message \'{} with ID \'{}\' is in the wrong ID space. Please use any of the available IDs from 0 to 169'.format(k, v) for k, v in list(incorrect_base_ids.items())))) - - if len(incorrect_alias_ids) > 0: - raise AssertionError( - ('\n' + '\n'.join('\t- The alias message \'{}\' with ID \'{}\' is in the wrong ID space. Please use any of the available IDs from 170 to 255'.format(k, v) for k, v in list(incorrect_alias_ids.items())))) - @staticmethod - def parse_yaml_msg_id_file(yaml_file): - """ - Parses a yaml file into a dict - """ + def parse_yaml_msgs_file(yaml_file) -> dict: + """Parses a yaml file into a dict.""" try: - with open(yaml_file, 'r') as f: - return yaml.safe_load(f) - except OSError as e: - if e.errno == errno.ENOENT: + with open(yaml_file, 'r') as file: + return yaml.safe_load(file) + except OSError as err: + if err.errno == errno.ENOENT: raise IOError(errno.ENOENT, os.strerror( errno.ENOENT), yaml_file) - else: - raise + raise if __name__ == "__main__": @@ -233,8 +146,8 @@ def parse_yaml_msg_id_file(yaml_file): parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, help="Topics message dir, by default msg/", default="msg") parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str, - help="RTPS msg IDs definition file absolute path, by default use relative path to msg, tools/uorb_rtps_message_ids.yaml", - default='tools/uorb_rtps_message_ids.yaml') + help="RTPS msg IDs definition file absolute path, by default use relative path to msg, tools/urtps_bridge_topics.yaml", + default='tools/urtps_bridge_topics.yaml') # Parse arguments args = parser.parse_args() @@ -250,36 +163,24 @@ def parse_yaml_msg_id_file(yaml_file): if args.send: if args.path: print(('send files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msgs_files_send) + '\n')) + for msg_file in classifier.msg_files_send) + '\n')) else: if args.alias: print((', '.join(str(msg) - for msg in list(classifier.msgs_to_send.keys())) + (' alias ' + ', '.join(str(list(msg[0].keys())[0]) - for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n')) + for msg in sorted(classifier.msgs_to_send)) + (' alias ' + ', '.join(msg[0] + for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n')) else: print((', '.join(str(msg) - for msg in list(classifier.msgs_to_send.keys())))) + for msg in sorted(classifier.msgs_to_send)))) if args.receive: if args.path: print(('receive files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msgs_files_receive) + '\n')) - else: - if args.alias: - print((', '.join(str(msg) - for msg in list(classifier.msgs_to_receive.keys())) + (' alias ' + ', '.join(str(list(msg[0].keys())[0]) - for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n')) - else: - print((', '.join(str(msg) - for msg in list(classifier.msgs_to_receive.keys())))) - if args.ignore: - if args.path: - print(('ignore files: ' + ', '.join(str(msg_file) - for msg_file in classifier.msgs_files_ignore) + '\n')) + for msg_file in classifier.msg_files_receive) + '\n')) else: if args.alias: print((', '.join(str(msg) - for msg in list(classifier.msgs_to_ignore.keys())) + (' alias ' + ', '.join(str(list(msg[0].keys())[0]) - for msg in classifier.alias_msgs_to_ignore) if len(classifier.alias_msgs_to_ignore) > 0 else '') + '\n')) + for msg in sorted(classifier.msgs_to_receive)) + (' alias ' + ', '.join(msg[0] + for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n')) else: print((', '.join(str(msg) - for msg in list(classifier.msgs_to_ignore.keys())))) + for msg in sorted(classifier.msgs_to_receive)))) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml deleted file mode 100644 index 5475d5d677ac..000000000000 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ /dev/null @@ -1,452 +0,0 @@ -rtps: - - msg: actuator_armed - id: 0 - - msg: actuator_controls - id: 1 - - msg: actuator_outputs - id: 3 - - msg: adc_report - id: 4 - - msg: airspeed - id: 5 - - msg: battery_status - id: 6 - - msg: camera_capture - id: 7 - - msg: camera_trigger - id: 8 - - msg: collision_report - id: 9 - - msg: commander_state - id: 10 - - msg: cpuload - id: 11 - - msg: debug_array - id: 12 - receive: true - - msg: debug_key_value - id: 13 - receive: true - - msg: debug_value - id: 14 - receive: true - - msg: debug_vect - id: 15 - receive: true - - msg: differential_pressure - id: 16 - - msg: distance_sensor - id: 17 - - msg: estimator_innovations - id: 18 - - msg: ekf2_timestamps - id: 19 - - msg: ekf_gps_drift - id: 20 - - msg: sensor_gps - id: 21 - - msg: esc_report - id: 22 - - msg: esc_status - id: 23 - - msg: estimator_status - id: 24 - - msg: follow_target - id: 25 - - msg: geofence_result - id: 26 - - msg: gps_dump - id: 27 - - msg: gps_inject_data - id: 28 - - msg: home_position - id: 29 - - msg: input_rc - id: 30 - send: true - - msg: iridiumsbd_status - id: 31 - - msg: irlock_report - id: 32 - - msg: landing_target_innovations - id: 33 - - msg: landing_target_pose - id: 34 - - msg: led_control - id: 35 - - msg: log_message - id: 36 - - msg: manual_control_setpoint - id: 37 - - msg: mavlink_log - id: 38 - - msg: mission - id: 39 - - msg: mission_result - id: 40 - - msg: mount_orientation - id: 41 - - msg: multirotor_motor_limits - id: 42 - - msg: obstacle_distance - id: 43 - - msg: offboard_control_mode - id: 44 - receive: true - - msg: optical_flow - id: 45 - receive: true - - msg: parameter_update - id: 46 - - msg: ping - id: 47 - - msg: position_controller_landing_status - id: 48 - - msg: position_controller_status - id: 49 - - msg: position_setpoint - id: 50 - receive: true - - msg: position_setpoint_triplet - id: 51 - receive: true - - msg: power_button_state - id: 52 - - msg: pwm_input - id: 53 - - msg: qshell_req - id: 54 - - msg: qshell_retval - id: 55 - - msg: radio_status - id: 56 - - msg: rate_ctrl_status - id: 57 - - msg: rc_channels - id: 58 - - msg: rc_parameter_map - id: 59 - - msg: safety - id: 60 - - msg: satellite_info - id: 61 - send: true - - msg: sensor_accel - id: 62 - - msg: sensor_baro - id: 63 - - msg: estimator_sensor_bias - id: 64 - - msg: sensor_combined - id: 65 - send: true - - msg: sensor_correction - id: 66 - - msg: sensor_gyro - id: 67 - - msg: sensor_mag - id: 68 - - msg: sensors_status_imu - id: 69 - - msg: sensor_selection - id: 70 - - msg: px4io_status - id: 71 - - msg: system_power - id: 73 - - msg: task_stack_info - id: 74 - - msg: tecs_status - id: 75 - - msg: telemetry_status - id: 76 - receive: true - - msg: test_motor - id: 77 - - msg: timesync - id: 78 - receive: true - send: true - - msg: trajectory_waypoint - id: 79 - receive: true - send: true - - msg: transponder_report - id: 80 - - msg: tune_control - id: 81 - - msg: uavcan_parameter_request - id: 82 - - msg: uavcan_parameter_value - id: 83 - - msg: ulog_stream - id: 84 - - msg: ulog_stream_ack - id: 85 - - msg: vehicle_air_data - id: 86 - - msg: vehicle_attitude - id: 87 - send: true - - msg: vehicle_attitude_setpoint - id: 88 - - msg: vehicle_command - id: 89 - receive: true - - msg: vehicle_command_ack - id: 90 - - msg: vehicle_constraints - id: 91 - - msg: vehicle_control_mode - id: 92 - send: true - - msg: vehicle_global_position - id: 93 - - msg: vehicle_gps_position - id: 94 - - msg: vehicle_land_detected - id: 95 - - msg: vehicle_local_position - id: 96 - send: true - - msg: vehicle_local_position_setpoint - id: 97 - receive: true - - msg: vehicle_magnetometer - id: 98 - - msg: vehicle_odometry - id: 99 - send: true - - msg: vehicle_rates_setpoint - id: 100 - - msg: vehicle_roi - id: 101 - - msg: vehicle_status - id: 102 - send: true - - msg: vehicle_status_flags - id: 103 - - msg: vehicle_trajectory_waypoint - id: 104 - receive: true - - msg: vtol_vehicle_status - id: 105 - - msg: wind - id: 106 - - msg: collision_constraints - id: 107 - send: true - - msg: orbit_status - id: 108 - - msg: power_monitor - id: 109 - - msg: landing_gear - id: 110 - - msg: wheel_encoders - id: 111 - - msg: vehicle_angular_velocity - id: 112 - send: true - - msg: vehicle_acceleration - id: 113 - - msg: airspeed_validated - id: 115 - - msg: onboard_computer_status - id: 116 - receive: true - - msg: cellular_status - id: 117 - - msg: sensor_accel_fifo - id: 118 - - msg: sensor_gyro_fifo - id: 119 - - msg: vehicle_imu - id: 120 - - msg: vehicle_imu_status - id: 121 - - msg: vehicle_angular_acceleration - id: 122 - - msg: logger_status - id: 123 - - msg: rpm - id: 124 - - msg: hover_thrust_estimate - id: 125 - - msg: trajectory_bezier - id: 126 - receive: true - - msg: vehicle_trajectory_bezier - id: 127 - receive: true - - msg: timesync_status - id: 128 - - msg: orb_test - id: 129 - - msg: orb_test_medium - id: 130 - - msg: orb_test_large - id: 131 - - msg: yaw_estimator_status - id: 132 - - msg: sensor_preflight_mag - id: 133 - - msg: estimator_states - id: 134 - - msg: generator_status - id: 135 - - msg: sensor_gyro_fft - id: 136 - - msg: navigator_mission_item - id: 137 - - msg: estimator_optical_flow_vel - id: 138 - - msg: estimator_selector_status - id: 139 - - msg: manual_control_switches - id: 140 - - msg: estimator_status_flags - id: 141 - - msg: rtl_flight_time - id: 142 - - msg: vehicle_angular_acceleration_setpoint - id: 143 - - msg: vehicle_torque_setpoint - id: 144 - - msg: vehicle_thrust_setpoint - id: 145 - - msg: vehicle_actuator_setpoint - id: 146 - - msg: control_allocator_status - id: 147 - - msg: mag_worker_data - id: 148 - - msg: takeoff_status - id: 149 - - msg: heater_status - id: 150 - - msg: gimbal_device_attitude_status - id: 151 - - msg: gimbal_device_information - id: 152 - - msg: gimbal_device_set_attitude - id: 153 - - msg: gimbal_manager_information - id: 154 - - msg: gimbal_manager_set_attitude - id: 155 - - msg: gimbal_manager_status - id: 156 - - msg: gimbal_manager_set_manual_control - id: 157 - - msg: airspeed_wind - id: 158 - - msg: estimator_event_flags - id: 159 - ########## multi topics: begin ########## - - msg: actuator_controls_0 - id: 170 - alias: actuator_controls - - msg: actuator_controls_1 - id: 171 - alias: actuator_controls - - msg: actuator_controls_2 - id: 172 - alias: actuator_controls - - msg: actuator_controls_3 - id: 173 - alias: actuator_controls - - msg: actuator_controls_virtual_fw - id: 174 - alias: actuator_controls - - msg: actuator_controls_virtual_mc - id: 175 - alias: actuator_controls - - msg: mc_virtual_attitude_setpoint - id: 176 - alias: vehicle_attitude_setpoint - - msg: fw_virtual_attitude_setpoint - id: 177 - alias: vehicle_attitude_setpoint - - msg: vehicle_attitude_groundtruth - id: 178 - alias: vehicle_attitude - - msg: vehicle_global_position_groundtruth - id: 179 - alias: vehicle_global_position - - msg: vehicle_local_position_groundtruth - id: 180 - alias: vehicle_local_position - - msg: vehicle_mocap_odometry - alias: vehicle_odometry - id: 181 - receive: true - - msg: vehicle_visual_odometry - id: 182 - alias: vehicle_odometry - receive: true - - msg: vehicle_trajectory_waypoint_desired - id: 183 - alias: vehicle_trajectory_waypoint - send: true - - msg: obstacle_distance_fused - id: 184 - alias: obstacle_distance - - msg: vehicle_vision_attitude - id: 185 - alias: vehicle_attitude - - msg: trajectory_setpoint - id: 186 - alias: vehicle_local_position_setpoint - receive: true - - msg: camera_trigger_secondary - id: 187 - alias: camera_trigger - - msg: vehicle_angular_velocity_groundtruth - id: 188 - alias: vehicle_angular_velocity - - msg: estimator_visual_odometry_aligned - id: 189 - alias: vehicle_odometry - - msg: estimator_innovation_variances - id: 190 - alias: estimator_innovations - - msg: estimator_innovation_test_ratios - id: 191 - alias: estimator_innovations - - msg: orb_multitest - id: 192 - alias: orb_test - - msg: orb_test_medium_multi - id: 193 - alias: orb_test_medium - - msg: orb_test_medium_queue - id: 194 - alias: orb_test_medium - - msg: orb_test_medium_queue_poll - id: 195 - alias: orb_test_medium - - msg: orb_test_medium_wrap_around - id: 196 - alias: orb_test_medium - - msg: estimator_local_position - id: 197 - alias: vehicle_local_position - - msg: estimator_global_position - id: 198 - alias: vehicle_global_position - - msg: estimator_attitude - id: 199 - alias: vehicle_attitude - - msg: estimator_odometry - id: 200 - alias: vehicle_odometry - - msg: actuator_controls_4 - id: 201 - alias: actuator_controls - - msg: actuator_controls_5 - id: 202 - alias: actuator_controls - - msg: estimator_wind - id: 203 - alias: wind - ########## multi topics: end ########## diff --git a/msg/tools/uorb_to_ros_msgs.py b/msg/tools/uorb_to_ros_msgs.py index 66876dd7a6a5..a0d2aa9fb324 100755 --- a/msg/tools/uorb_to_ros_msgs.py +++ b/msg/tools/uorb_to_ros_msgs.py @@ -53,62 +53,75 @@ input_dir = sys.argv[1] output_dir = sys.argv[2] -if not os.path.exists(os.path.abspath(output_dir)): - os.mkdir(os.path.abspath(output_dir)) -else: - ros_msg_dir = os.path.abspath(output_dir) - msg_files = os.listdir(ros_msg_dir) - for msg in msg_files: - if msg.endswith(".msg"): - os.remove(os.path.join(ros_msg_dir, msg)) - -msg_list = list() - -for filename in os.listdir(input_dir): - if '.msg' in filename: - msg_list.append(filename.rstrip('.msg')) - input_file = input_dir + filename - - output_file = output_dir + \ - filename.partition(".")[0].title().replace('_', '') + ".msg" - copyfile(input_file, output_file) - -for filename in os.listdir(output_dir): - if '.msg' in filename: - input_file = output_dir + filename - - fileUpdated = False - - with open(input_file, 'r') as f: - lines = f.readlines() - newlines = [] - alias_msgs = [] - alias_msg_files = [] - - for line in lines: - for msg_type in msg_list: - if ('px4/' + msg_type + ' ') in line: - fileUpdated = True - line = line.replace(('px4/' + msg_type), - msg_type.partition(".")[0].title().replace('_', '')) - if re.findall('^' + msg_type + '[\s\[]', line.partition('#')[0]): - fileUpdated = True - line = line.replace(msg_type, - msg_type.partition(".")[0].title().replace('_', '')) - if '# TOPICS' in line: - fileUpdated = True - alias_msgs += line.split() - alias_msgs.remove('#') - alias_msgs.remove('TOPICS') - line = line.replace(line, '') - newlines.append(line) - - for msg_file in alias_msgs: - with open(output_dir + msg_file.partition(".")[0].title().replace('_', '') + ".msg", 'w+') as f: - for line in newlines: - f.write(line) - - if fileUpdated: - with open(input_file, 'w+') as f: - for line in newlines: - f.write(line) + +def main(): + print("----------------------- \033[1mmicroRTPS bridge uORB to ROS messages\033[0m -----------------------") + print("-------------------------------------------------------------------------------------------------------") + + if not os.path.exists(os.path.abspath(output_dir)): + os.mkdir(os.path.abspath(output_dir)) + else: + ros_msg_dir = os.path.abspath(output_dir) + msg_files = os.listdir(ros_msg_dir) + for msg in msg_files: + if msg.endswith(".msg"): + os.remove(os.path.join(ros_msg_dir, msg)) + + msg_list = list() + + for filename in os.listdir(input_dir): + if '.msg' in filename: + msg_list.append(filename.rstrip('.msg')) + input_file = input_dir + filename + + output_file = output_dir + \ + filename.partition(".")[0].title().replace('_', '') + ".msg" + copyfile(input_file, output_file) + + for filename in os.listdir(output_dir): + if '.msg' in filename: + input_file = output_dir + filename + + fileUpdated = False + + with open(input_file, 'r') as f: + lines = f.readlines() + newlines = [] + alias_msgs = [] + alias_msg_files = [] + + for line in lines: + for msg_type in msg_list: + if ('px4/' + msg_type + ' ') in line: + fileUpdated = True + line = line.replace(('px4/' + msg_type), + msg_type.partition(".")[0].title().replace('_', '')) + + if re.findall('^' + msg_type + '[\s\[]', line.partition('#')[0]): + fileUpdated = True + line = line.replace(msg_type, + msg_type.partition(".")[0].title().replace('_', '')) + if '# TOPICS' in line: + fileUpdated = True + alias_msgs += line.split() + alias_msgs.remove('#') + alias_msgs.remove('TOPICS') + line = line.replace(line, '') + newlines.append(line) + + for msg_file in alias_msgs: + with open(output_dir + msg_file.partition(".")[0].title().replace('_', '') + ".msg", 'w+') as f: + for line in newlines: + f.write(line) + + if fileUpdated: + with open(input_file, 'w+') as f: + for line in newlines: + f.write(line) + + print("--\t\t- Generated {} ROS message files in '{}'".format(len(os.listdir(output_dir)), os.path.abspath(output_dir))) + print("-------------------------------------------------------------------------------------------------------") + + +if __name__ == '__main__': + main() diff --git a/msg/tools/uorb_to_ros_rtps_ids.py b/msg/tools/uorb_to_ros_rtps_ids.py deleted file mode 100755 index 44d5a6ef7965..000000000000 --- a/msg/tools/uorb_to_ros_rtps_ids.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python3 -""" -Script to read an yaml file containing the RTPS message IDs and update the naming convention to PascalCase -""" - -import errno -import os -import yaml -import sys -import argparse -import six - -__author__ = 'PX4 Development Team' -__copyright__ = \ - ''' - ' - ' Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - ' - ' Redistribution and use in source and binary forms, or without - ' modification, permitted provided that the following conditions - ' are met: - ' - ' 1. Redistributions of source code must retain the above copyright - ' notice, list of conditions and the following disclaimer. - ' 2. Redistributions in binary form must reproduce the above copyright - ' notice, 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 self 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, NOT - ' LIMITED TO, 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, CONSEQUENTIAL DAMAGES (INCLUDING, - ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS - ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT - ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE - ' POSSIBILITY OF SUCH DAMAGE. - ' - ''' -__credits__ = ['Nuno Marques '] -__license__ = 'BSD-3-Clause' -__version__ = '0.1.0' -__maintainer__ = 'Nuno Marques' -__email__ = 'nuno.marques@dronesolution.io' -__status__ = 'Development' - -list = [] -in_file = "" -out_file = "" -verbose = True - - -class IndenterDumper(yaml.Dumper): - """ Custom dumper for yaml files that apply the correct indentation """ - - def increase_indent(self, flow=False, indentless=False): - return super(IndenterDumper, self).increase_indent(flow, False) - - -def load_yaml_file(file): - """ - Open yaml file and parse the data into a list of dict - - :param file: the yaml file to load - :returns: the list of dictionaries that represent the message and respective RTPS IDs - :raises IOError: raises and error when the file is not found - """ - try: - with open(file, 'r') as f: - if verbose: - print(("--\t[Step 1] %s yaml file loaded!" % file)) - return yaml.safe_load(f) - except OSError as e: - if e.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), file) - else: - raise - - -def update_dict(list): - """ - Update the message naming on the dictionary to fit the PascalCase convention - - :param file: the list of dicts to be updated - """ - if verbose: - num_of_msgs = 0 - for i, dictionary in enumerate(list["rtps"]): - list["rtps"][i] = {k: v.title().replace('_', '') if isinstance( - v, six.string_types) else v for k, v in six.iteritems(dictionary)} - if verbose: - num_of_msgs += 1 - if verbose: - print(("--\t[Step 2] List: %d msg names updated!" % num_of_msgs)) - - -def update_yaml_file(list, file): - """ - Open the yaml file to dump the new list of dict toself. - - .. note:: Since the the dump method automatically sorts the keys alphabetically, - the 'id' fields will appear first than the 'msg' fields. - - :param list: the list of updated dicts - :param file: the yaml file to load and write the new data - :raises IOError: raises and error when the file is not found - """ - try: - with open(file, 'w') as f: - yaml.dump(list, f, Dumper=IndenterDumper, default_flow_style=False) - if verbose: - if in_file == out_file: - print(("--\t[Step 3] %s updated!" % in_file)) - else: - print(("--\t[Step 3] %s created!" % out_file)) - - except OSError as e: - if e.errno == errno.ENOENT: - raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), file) - else: - raise - - -if __name__ == "__main__": - parser = argparse.ArgumentParser( - description='Read an yaml file containing the RTPS message IDs and update the naming convention to PascalCase') - optional = parser._action_groups.pop() - required = parser.add_argument_group('Required') - required.add_argument("-i", "--input-file", dest="input_file", - help="Yaml file to read", metavar="INFILE") - optional.add_argument("-o", "--output-file", dest="output_file", - help="Yaml file to dump. If not set, it is the same as the input", - metavar="OUTFILE", default="") - optional.add_argument("-q", "--quiet", action="store_false", dest="verbose", - default=True, help="Don't print status messages to stdout") - - args = parser.parse_args() - verbose = args.verbose - in_file = args.input_file - out_file = args.output_file if (args.output_file != in_file and args.output_file != "") else in_file - - if verbose: - print("-- PX4 to ROS RTPS Ids --") - - list = load_yaml_file(in_file) - update_dict(list) - update_yaml_file(list, out_file) diff --git a/msg/tools/uorb_to_ros_urtps_topics.py b/msg/tools/uorb_to_ros_urtps_topics.py new file mode 100755 index 000000000000..8ff5f0e59dd5 --- /dev/null +++ b/msg/tools/uorb_to_ros_urtps_topics.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +""" +Script to read an yaml file containing the microRTPS topics and update the naming convention to PascalCase +""" + +import argparse +import errno +from pathlib import Path +import os +import six +import yaml + +__author__ = 'PX4 Development Team' +__copyright__ = \ + ''' + ' + ' Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + ' + ' Redistribution and use in source and binary forms, or without + ' modification, permitted provided that the following conditions + ' are met: + ' + ' 1. Redistributions of source code must retain the above copyright + ' notice, list of conditions and the following disclaimer. + ' 2. Redistributions in binary form must reproduce the above copyright + ' notice, 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 self 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, NOT + ' LIMITED TO, 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, CONSEQUENTIAL DAMAGES (INCLUDING, + ' BUT NOT LIMITED TO, OF SUBSTITUTE GOODS OR SERVICES; LOSS + ' OF USE, DATA, PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + ' AND ON ANY THEORY OF LIABILITY, IN CONTRACT, STRICT + ' LIABILITY, TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ' ANY WAY OUT OF THE USE OF THIS SOFTWARE, IF ADVISED OF THE + ' POSSIBILITY OF SUCH DAMAGE. + ' + ''' +__credits__ = ['Nuno Marques '] +__license__ = 'BSD-3-Clause' +__version__ = '0.1.0' +__maintainer__ = 'Nuno Marques' +__email__ = 'nuno.marques@dronesolution.io' +__status__ = 'Development' + +verbose = False + + +class IndenterDumper(yaml.Dumper): + """ Custom dumper for yaml files that apply the correct indentation """ + + def increase_indent(self, flow=False, indentless=False): + return super(IndenterDumper, self).increase_indent(flow, False) + + +def load_yaml_file(file): + """ + Open yaml file and parse the data into a list of dict + + :param file: the yaml file to load + :returns: the list of dictionaries that represent the topics to send and receive + :raises IOError: raises and error when the file is not found + """ + try: + with open(file, 'r') as f: + if verbose: + print(("--\t\t- '%s' file loaded" % file)) + return yaml.safe_load(f) + except OSError as e: + if e.errno == errno.ENOENT: + raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), file) + else: + raise + + +def update_dict(list): + """ + Update the message naming on the dictionary to fit the PascalCase convention + + :param file: the list of dicts to be updated + """ + if verbose: + num_of_msgs = 0 + for i, dictionary in enumerate(list["rtps"]): + list["rtps"][i] = {k: v.title().replace('_', '') if isinstance( + v, six.string_types) else v for k, v in six.iteritems(dictionary)} + if verbose: + num_of_msgs += 1 + if verbose: + print(("--\t\t- %d ROS message file names updated" % num_of_msgs)) + + +def update_yaml_file(list, in_file, out_file): + """ + Open the yaml file to dump the new list of dict toself. + + :param list: the list of updated dicts + :param file: the yaml file to load and write the new data + :raises IOError: raises and error when the file is not found + """ + try: + with open(out_file, 'w') as f: + f.write("# AUTOGENERATED-FILE! DO NOT MODIFY IT DIRECTLY.\n#" + " Edit instead the same file under PX4-Autopilot/msg/tools and" + " use the \n# PX4-Autopilot/msg/tools/uorb_to_ros_urtps_topics.py" + " to regenerate this file.\n") + yaml.dump(list, f, Dumper=IndenterDumper, default_flow_style=False) + if verbose: + if in_file == out_file: + print(("--\t\t- '%s' updated" % in_file)) + else: + print(("--\t\t- '%s' created" % out_file)) + + except OSError as err: + if err.errno == errno.ENOENT: + raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), out_file) + raise + + +def main(): + parser = argparse.ArgumentParser( + description='Read an yaml file containing the microRTPS topics and update the naming convention to PascalCase') + optional = parser._action_groups.pop() + required = parser.add_argument_group('Required') + required.add_argument("-i", "--input-file", dest="input_file", + help="Yaml file to read", metavar="INFILE") + optional.add_argument("-o", "--output-file", dest="output_file", + help="Yaml file to dump. If not set, it is the same as the input", + metavar="OUTFILE", default="") + optional.add_argument("-q", "--quiet", action="store_false", dest="verbose", + default=True, help="Don't print status messages to stdout") + + args = parser.parse_args() + global verbose + verbose = args.verbose + in_file = Path(args.input_file) + out_file = Path(args.output_file) if ( + Path(args.output_file) != in_file and Path(args.output_file) != "") else in_file + + if verbose: + print("---------------------- \033[1mmicroRTPS bridge yaml PX4 to ROS topics\033[0m ----------------------") + print("-------------------------------------------------------------------------------------------------------") + + list = load_yaml_file(in_file) + update_dict(list) + update_yaml_file(list, in_file, out_file) + if verbose: + print("-------------------------------------------------------------------------------------------------------") + + +if __name__ == "__main__": + main() diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml new file mode 100644 index 000000000000..529811ce5b28 --- /dev/null +++ b/msg/tools/urtps_bridge_topics.yaml @@ -0,0 +1,90 @@ +##### +# +# This file maps all the topics that are to be used on the microRTPS bridge. +# When one wants to add a new topic to the bridge, it should add it to this file +# and mark it to be sent or received from the link. +# For alias/multi-topic messages (i.e. the ones found on the '#TOPICS' of the +# uORB messages), these can be also added, requiring an extra entry ('base') to +# define the base message. +# +# IMPORTANT NOTICE: The IDs of the messages sent on the bridge get generated +# according to the order of the messages in this file. To keep consistency and +# backwards compatibility, it is recommended that any new message that one wants +# to be streamed in the bridge gets added to the end of the list. Any changes +# in the middle of the list (additions, removals, replacements) will change also +# the current message IDS, which might result with incompatibilities with +# previous PX4 versions (where the list with this format got introduced or +# subsisted). +# +# Any updates to this file should be mirrored in both sides of the bridge (i.e., +# PX4 and px4_ros_com), when using it with ROS2. That can be easily done using +# the 'msg/tools/uorb_to_ros_urtps_topics.py' script to regenerate this same +# file under 'px4_ros_com/templates/''. The same is not applicable/required if +# using this bridge with "raw" RTPS/DDS applications, since the microRTPS agent +# to be used and stored in 'build//src/modules/micrortps_bridge/'' +# gets generated using this same list. +# +##### +rtps: + # topic ID 1 + - msg: debug_array + receive: true + # topic ID 2 + - msg: debug_key_value + receive: true + # topic ID 3 + - msg: debug_value + receive: true + # ... + - msg: debug_vect + receive: true + - msg: offboard_control_mode + receive: true + - msg: sensor_optical_flow + receive: true + - msg: position_setpoint + receive: true + - msg: position_setpoint_triplet + receive: true + - msg: telemetry_status + receive: true + - msg: timesync + receive: true + send: true + - msg: trajectory_waypoint + send: true + - msg: vehicle_command + receive: true + - msg: vehicle_control_mode + send: true + - msg: vehicle_local_position_setpoint + receive: true + - msg: trajectory_setpoint + receive: true + - msg: vehicle_odometry + send: true + - msg: vehicle_mocap_odometry # multi-topic / alias of vehicle_odometry + base: vehicle_odometry + receive: true + - msg: vehicle_visual_odometry # multi-topic / alias of vehicle_odometry + base: vehicle_odometry + receive: true + - msg: vehicle_status + send: true + - msg: vehicle_trajectory_waypoint + receive: true + - msg: vehicle_trajectory_waypoint_desired # multi-topic / alias of vehicle_trajectory_waypoint + base: vehicle_trajectory_waypoint + send: true + - msg: collision_constraints + send: true + - msg: onboard_computer_status + receive: true + - msg: trajectory_bezier + receive: true + - msg: vehicle_trajectory_bezier + receive: true + - msg: timesync_status + send: true + - msg: sensor_combined + send: true diff --git a/msg/trajectory_setpoint.msg b/msg/trajectory_setpoint.msg new file mode 100644 index 000000000000..4a88c86769a6 --- /dev/null +++ b/msg/trajectory_setpoint.msg @@ -0,0 +1,15 @@ +# Trajectory setpoint in NED frame +# Input to PID position controller. +# Needs to be kinematically consistent and feasible for smooth flight. +# setting a value to NaN means the state should not be controlled + +uint64 timestamp # time since system start (microseconds) + +# NED local world frame +float32[3] position # in meters +float32[3] velocity # in meters/second +float32[3] acceleration # in meters/second^2 +float32[3] jerk # in meters/second^3 (for logging only) + +float32 yaw # euler angle of desired attitude in radians -PI..+PI +float32 yawspeed # angular velocity around NED frame z-axis in radians/second diff --git a/msg/tune_control.msg b/msg/tune_control.msg index dba37757d21c..96d70af15423 100644 --- a/msg/tune_control.msg +++ b/msg/tune_control.msg @@ -22,7 +22,8 @@ uint8 TUNE_ID_SD_ERROR = 15 uint8 TUNE_ID_PROG_PX4IO = 16 uint8 TUNE_ID_PROG_PX4IO_OK = 17 uint8 TUNE_ID_PROG_PX4IO_ERR = 18 -uint8 NUMBER_OF_TUNES = 19 +uint8 TUNE_ID_POWER_OFF = 19 +uint8 NUMBER_OF_TUNES = 20 uint8 tune_id # tune_id corresponding to TuneID::* from the tune_defaults.h in the tunes library bool tune_override # if true the tune which is playing will be stopped and the new started @@ -32,7 +33,7 @@ uint32 silence # in us uint8 volume # value between 0-100 if supported by backend uint8 VOLUME_LEVEL_MIN = 0 -uint8 VOLUME_LEVEL_DEFAULT = 40 +uint8 VOLUME_LEVEL_DEFAULT = 20 uint8 VOLUME_LEVEL_MAX = 100 uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/uwb_distance.msg b/msg/uwb_distance.msg new file mode 100644 index 000000000000..8f1f7105b575 --- /dev/null +++ b/msg/uwb_distance.msg @@ -0,0 +1,15 @@ +# UWB distance contains the distance information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) +uint32 time_uwb_ms # Time of UWB device in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint32 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint16 status # status feedback # + +uint16[12] anchor_distance # distance in cm to each UWB Anchor (UWB Device wich takes part in Ranging) +bool[12] nlos # Visual line of sight yes/no +float32[12] aoafirst # Angle of arrival of first incomming RX msg + +float32[3] position # Position of the Landing point in relation to the Drone (x,y,z in Meters NED) diff --git a/msg/uwb_grid.msg b/msg/uwb_grid.msg new file mode 100644 index 000000000000..0862f84330c7 --- /dev/null +++ b/msg/uwb_grid.msg @@ -0,0 +1,25 @@ +# UWB report message contains the grid information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) +uint16 initator_time # time to synchronize clocks (microseconds) +uint8 num_anchors # Number of anchors + +float64[4] target_gps # GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North) + +# Grid position information +# Position in x,y,z in (x,y,z in centimeters NED) +# staring with POI and Anchor 0 up to Anchor 11 +int16[3] target_pos # Point of Interest, mostly landing Target x,y,z +int16[3] anchor_pos_0 +int16[3] anchor_pos_1 +int16[3] anchor_pos_2 +int16[3] anchor_pos_3 +int16[3] anchor_pos_4 +int16[3] anchor_pos_5 +int16[3] anchor_pos_6 +int16[3] anchor_pos_7 +int16[3] anchor_pos_8 +int16[3] anchor_pos_9 +int16[3] anchor_pos_10 +int16[3] anchor_pos_11 diff --git a/msg/vehicle_actuator_setpoint.msg b/msg/vehicle_actuator_setpoint.msg deleted file mode 100644 index 54c0c9b61f8e..000000000000 --- a/msg/vehicle_actuator_setpoint.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled - -uint8 NUM_ACTUATOR_SETPOINT = 16 - -float32[16] actuator diff --git a/msg/vehicle_air_data.msg b/msg/vehicle_air_data.msg index 4138c73ad3df..c1e55c163185 100644 --- a/msg/vehicle_air_data.msg +++ b/msg/vehicle_air_data.msg @@ -10,3 +10,5 @@ float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals float32 rho # air density + +uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index a57b7c50586e..f704d7193357 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,12 +1,12 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame -float32[4] delta_q_reset # Amount by which quaternion has changed during last reset -uint8 quat_reset_counter # Quaternion reset counter +float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude # TOPICS estimator_attitude diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 065fa12314eb..aeb0f2aeddeb 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -24,4 +24,9 @@ uint8 FLAPS_OFF = 0 # no flaps uint8 FLAPS_LAND = 1 # landing config flaps uint8 FLAPS_TAKEOFF = 2 # take-off config flaps +uint8 apply_spoilers # spoiler config specifier +uint8 SPOILERS_OFF = 0 # no spoilers +uint8 SPOILERS_LAND = 1 # landing config spoiler +uint8 SPOILERS_DESCEND = 2 # descend config spoiler + # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 089934f1035c..ffe10ea9d0e6 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -36,8 +36,9 @@ uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desir uint16 VEHICLE_CMD_DO_SET_SERVO = 183 # Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ -uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ +uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| +uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| +uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| uint16 VEHICLE_CMD_DO_REPOSITION = 192 uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| @@ -68,7 +69,9 @@ uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto M uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| -uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| +uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| +uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message @@ -77,7 +80,10 @@ uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control +uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence. uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500 # Start a video capture. +uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture. uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link @@ -120,6 +126,10 @@ uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1 # Continuous zoom up/down unt uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2 # Zoom value as proportion of full camera range uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3 # Zoom to a focal length +uint8 PARACHUTE_ACTION_DISABLE = 0 +uint8 PARACHUTE_ACTION_ENABLE = 1 +uint8 PARACHUTE_ACTION_RELEASE = 2 + uint8 FAILURE_UNIT_SENSOR_GYRO = 0 uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 uint8 FAILURE_UNIT_SENSOR_MAG = 2 @@ -145,6 +155,16 @@ uint8 FAILURE_TYPE_SLOW = 5 uint8 FAILURE_TYPE_DELAYED = 6 uint8 FAILURE_TYPE_INTERMITTENT = 7 +# used as param1 in DO_CHANGE_SPEED command +uint8 SPEED_TYPE_AIRSPEED = 0 +uint8 SPEED_TYPE_GROUNDSPEED = 1 +uint8 SPEED_TYPE_CLIMB_SPEED = 2 +uint8 SPEED_TYPE_DESCEND_SPEED = 3 + +# used as param1 in ARM_DISARM command +int8 ARMING_ACTION_DISARM = 0 +int8 ARMING_ACTION_ARM = 1 + uint8 ORB_QUEUE_LENGTH = 8 float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. @@ -161,3 +181,5 @@ uint8 source_system # System sending the command uint8 source_component # Component sending the command uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) bool from_external + +# TOPICS vehicle_command gimbal_v1_command diff --git a/msg/vehicle_constraints.msg b/msg/vehicle_constraints.msg index 0e624c037063..aa3a491b125f 100644 --- a/msg/vehicle_constraints.msg +++ b/msg/vehicle_constraints.msg @@ -3,7 +3,6 @@ uint64 timestamp # time since system start (microseconds) -float32 speed_xy # in meters/sec float32 speed_up # in meters/sec float32 speed_down # in meters/sec diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 9592a694cae0..1f4034369d91 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -1,7 +1,7 @@ uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed -bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing +bool flag_multicopter_position_control_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index ef8402f2b046..a71bb7a01f5b 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -8,12 +8,16 @@ 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 uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 + +uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame -uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 149e0ac950e3..78fb44703ed9 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -4,6 +4,7 @@ uint32 accel_device_id # unique device ID for the sensor that does not uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles uint32[3] accel_clipping # total clipping per axis +uint32[3] gyro_clipping # total clipping per axis uint32 accel_error_count uint32 gyro_error_count @@ -14,12 +15,14 @@ float32 gyro_rate_hz float32 accel_raw_rate_hz # full raw sensor sample rate (Hz) float32 gyro_raw_rate_hz # full raw sensor sample rate (Hz) -float32 accel_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) -float32 gyro_vibration_metric # high frequency vibration level in the IMU delta velocity data (m/s) -float32 gyro_coning_vibration # Level of coning vibration in the IMU delta angles (rad^2) +float32 accel_vibration_metric # high frequency vibration level in the accelerometer data (m/s/s) +float32 gyro_vibration_metric # high frequency vibration level in the gyro data (rad/s) +float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2) float32[3] mean_accel # average accelerometer readings since last publication float32[3] mean_gyro # average gyroscope readings since last publication +float32[3] var_accel # accelerometer variance since last publication +float32[3] var_gyro # gyroscope variance since last publication float32 temperature_accel float32 temperature_gyro diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg index 6f07cf497c5c..a35a8414c362 100644 --- a/msg/vehicle_land_detected.msg +++ b/msg/vehicle_land_detected.msg @@ -1,7 +1,18 @@ uint64 timestamp # time since system start (microseconds) -float32 alt_max # maximum altitude in [m] that can be reached + bool freefall # true if vehicle is currently in free-fall bool ground_contact # true if vehicle has ground contact but is not landed (1. stage) bool maybe_landed # true if the vehicle might have landed (2. stage) bool landed # true if vehicle is currently landed on the ground (3. stage) + bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing). +bool in_descend + +bool has_low_throttle + +bool vertical_movement +bool horizontal_movement + +bool close_to_ground_or_skipped_check + +bool at_rest diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 6f42547e43e6..981fec4ba883 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,11 +1,12 @@ # Fused local position in NED. +# The coordinate system origin is the vehicle position at the time when the EKF2-module was started. uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) bool xy_valid # true if x and y are valid bool z_valid # true if z is valid -bool v_xy_valid # true if vy and vy are valid +bool v_xy_valid # true if vx and vy are valid bool v_z_valid # true if vz is valid # Position in local NED frame @@ -40,6 +41,7 @@ float32 az # Down velocity derivative in NED earth-fixed frame, (metres/s float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) float32 delta_heading uint8 heading_reset_counter +bool heading_good_for_control # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) diff --git a/msg/vehicle_local_position_setpoint.msg b/msg/vehicle_local_position_setpoint.msg index 418cf5052b85..0093d52d770b 100644 --- a/msg/vehicle_local_position_setpoint.msg +++ b/msg/vehicle_local_position_setpoint.msg @@ -1,18 +1,19 @@ # Local position setpoint in NED frame -# setting something to NaN means the state should not be controlled +# Telemetry of PID position controller to monitor tracking. +# NaN means the state was not controlled uint64 timestamp # time since system start (microseconds) float32 x # in meters NED float32 y # in meters NED float32 z # in meters NED -float32 yaw # in radians NED -PI..+PI -float32 yawspeed # in radians/sec + float32 vx # in meters/sec float32 vy # in meters/sec float32 vz # in meters/sec + float32[3] acceleration # in meters/sec^2 -float32[3] jerk # in meters/sec^3 float32[3] thrust # normalized thrust vector in NED -# TOPICS vehicle_local_position_setpoint trajectory_setpoint +float32 yaw # in radians NED -PI..+PI +float32 yawspeed # in radians/sec diff --git a/msg/vehicle_odometry.msg b/msg/vehicle_odometry.msg index 4e2a0d13f9c0..5f597f4d96a7 100644 --- a/msg/vehicle_odometry.msg +++ b/msg/vehicle_odometry.msg @@ -62,5 +62,7 @@ float32 yawspeed # Angular velocity about Z body axis # If angular velocity covariance invalid/unknown, 16th cell is NaN float32[21] velocity_covariance +uint8 reset_counter + # TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry # TOPICS estimator_odometry estimator_visual_odometry_aligned diff --git a/msg/vehicle_optical_flow.msg b/msg/vehicle_optical_flow.msg new file mode 100644 index 000000000000..13bdb57bbf25 --- /dev/null +++ b/msg/vehicle_optical_flow.msg @@ -0,0 +1,21 @@ +# Optical flow in XYZ body frame in SI units. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) + +float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/msg/vehicle_optical_flow_vel.msg b/msg/vehicle_optical_flow_vel.msg new file mode 100644 index 000000000000..c89207f4abeb --- /dev/null +++ b/msg/vehicle_optical_flow_vel.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) + +float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) +float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) + +float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) +float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) + +# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index fae2a015435b..902d07a478ba 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -1,5 +1,3 @@ -# If you change the order, add or remove arming_state_t states make sure to update the arrays -# in state_machine_helper.cpp as well. uint64 timestamp # time since system start (microseconds) uint8 ARMING_STATE_INIT = 0 @@ -11,12 +9,15 @@ uint8 ARMING_STATE_IN_AIR_RESTORE = 5 uint8 ARMING_STATE_MAX = 6 # FailureDetector status -uint8 FAILURE_NONE = 0 -uint8 FAILURE_ROLL = 1 # (1 << 0) -uint8 FAILURE_PITCH = 2 # (1 << 1) -uint8 FAILURE_ALT = 4 # (1 << 2) -uint8 FAILURE_EXT = 8 # (1 << 3) -uint8 FAILURE_ARM_ESC = 16 # (1 << 4) +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) # HIL uint8 HIL_STATE_OFF = 0 @@ -29,10 +30,10 @@ 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_AUTO_LANDENGFAIL = 8 # Auto land on engine failure -uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down) +uint8 NAVIGATION_STATE_UNUSED3 = 8 # Free slot +uint8 NAVIGATION_STATE_UNUSED = 9 # Free slot uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode -uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot +uint8 NAVIGATION_STATE_UNUSED1 = 11 # Free slot uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 @@ -43,11 +44,8 @@ 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_MAX = 22 - -uint8 RC_IN_MODE_DEFAULT = 0 -uint8 RC_IN_MODE_OFF = 1 -uint8 RC_IN_MODE_GENERATED = 2 +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_MAX = 23 uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 @@ -69,33 +67,32 @@ 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 -uint8 vehicle_type # Type of vehicle (fixed-wing, rotary wing, ground) +uint8 vehicle_type # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above) # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing 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 vtol_fw_permanent_stab # True if VTOL should stabilize attitude for fw in manual mode 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 bool rc_signal_lost # true if RC reception lost -uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. bool data_link_lost # datalink to GCS lost uint8 data_link_lost_counter # counts unique data link 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 -bool engine_failure # Set to true if an engine failure is detected bool mission_failure # Set to true if mission could not continue/finish +bool geofence_violated -uint8 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL] +uint16 failure_detector_status # Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL] # see SYS_STATUS mavlink message for the following -uint32 onboard_control_sensors_present -uint32 onboard_control_sensors_enabled -uint32 onboard_control_sensors_health +# lower 32 bits are for the base flags, higher 32 bits are or the extended flags +uint64 onboard_control_sensors_present +uint64 onboard_control_sensors_enabled +uint64 onboard_control_sensors_health uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 @@ -119,3 +116,6 @@ uint8 latest_disarming_reason uint64 armed_time # Arming timestamp (microseconds) uint64 takeoff_time # Takeoff timestamp (microseconds) + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index af9ca6609905..70b9adf46e39 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -2,35 +2,40 @@ uint64 timestamp # time since system start (microseconds) -bool condition_calibration_enabled -bool condition_system_sensors_initialized -bool condition_system_hotplug_timeout # true if the hotplug sensor search is over -bool condition_system_returned_to_home -bool condition_auto_mission_available -bool condition_angular_velocity_valid -bool condition_attitude_valid -bool condition_local_altitude_valid -bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation -bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation -bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation -bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) -bool condition_power_input_valid # set if input power is valid -bool condition_battery_healthy # set if battery is available and not low -bool condition_escs_error # set to true if one or more ESCs reporting esc_status are offline +bool calibration_enabled +bool pre_flight_checks_pass # true if all checks necessary to arm pass +bool auto_mission_available +bool angular_velocity_valid +bool attitude_valid +bool local_altitude_valid +bool local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation +bool local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation +bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation +bool gps_position_valid +bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool power_input_valid # set if input power is valid +bool battery_healthy # set if battery is available and not low +bool escs_error # set to true if one or more ESCs reporting esc_status are offline +bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure + +bool position_reliant_on_gps +bool position_reliant_on_optical_flow +bool position_reliant_on_vision_position + +bool dead_reckoning +bool flight_terminated bool circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check -bool circuit_breaker_engaged_enginefailure_check bool circuit_breaker_flight_termination_disabled bool circuit_breaker_engaged_usb_check bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed -bool offboard_control_signal_found_once bool offboard_control_signal_lost bool rc_signal_found_once -bool rc_input_blocked # set if RC input should be ignored temporarily +bool rc_calibration_in_progress bool rc_calibration_valid # set if RC calibration is valid bool vtol_transition_failure # Set to true if vtol transition failed bool usb_connected # status of the USB power supply @@ -38,3 +43,6 @@ bool sd_card_detected_once # set to true if the SD card w 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 parachute_system_present +bool parachute_system_healthy diff --git a/msg/vehicle_thrust_setpoint.msg b/msg/vehicle_thrust_setpoint.msg index 1c292b4c93e8..27c35726b9d8 100644 --- a/msg/vehicle_thrust_setpoint.msg +++ b/msg/vehicle_thrust_setpoint.msg @@ -2,4 +2,4 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) -float32[3] xyz # thrust setpoint along X, Y, Z body axis (in N) +float32[3] xyz # thrust setpoint along X, Y, Z body axis [-1, 1] diff --git a/msg/vehicle_torque_setpoint.msg b/msg/vehicle_torque_setpoint.msg index c2c3e71106e4..988d5835927c 100644 --- a/msg/vehicle_torque_setpoint.msg +++ b/msg/vehicle_torque_setpoint.msg @@ -2,4 +2,4 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) -float32[3] xyz # torque setpoint about X, Y, Z body axis (in N.m) +float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized) diff --git a/msg/vtol_vehicle_status.msg b/msg/vtol_vehicle_status.msg index 7bc79768b576..80c424e7a446 100644 --- a/msg/vtol_vehicle_status.msg +++ b/msg/vtol_vehicle_status.msg @@ -7,8 +7,6 @@ uint8 VEHICLE_VTOL_STATE_FW = 4 uint64 timestamp # time since system start (microseconds) -bool vtol_in_rw_mode # true: vtol vehicle is in rotating wing mode -bool vtol_in_trans_mode -bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW +uint8 vehicle_vtol_state # current state of the vtol, see VEHICLE_VTOL_STATE + bool vtol_transition_failsafe # vtol in transition failsafe mode -bool fw_permanent_stab # In fw mode stabilize attitude even if in manual mode diff --git a/msg/yaw_estimator_status.msg b/msg/yaw_estimator_status.msg index 4f6ab5895612..36091e26e75b 100644 --- a/msg/yaw_estimator_status.msg +++ b/msg/yaw_estimator_status.msg @@ -3,6 +3,7 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) float32 yaw_composite # composite yaw from GSF (rad) float32 yaw_variance # composite yaw variance from GSF (rad^2) +bool yaw_composite_valid float32[5] yaw # yaw estimate for each model in the filter bank (rad) float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 2e5dc931a6b9..120a59fdfe4c 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -33,7 +33,7 @@ set(SRCS) -if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") +if(NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") list(APPEND SRCS px4_log.cpp ) @@ -41,6 +41,7 @@ endif() add_library(px4_platform board_identity.c + events.cpp external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp @@ -51,11 +52,10 @@ add_library(px4_platform spi.cpp ${SRCS} ) -add_dependencies(px4_platform prebuild_targets) +target_link_libraries(px4_platform prebuild_targets px4_work_queue) if (NOT "${PX4_BOARD}" MATCHES "io-v2") add_subdirectory(uORB) - target_link_libraries(px4_platform PRIVATE uORB) endif() add_subdirectory(px4_work_queue) diff --git a/platforms/common/events.cpp b/platforms/common/events.cpp new file mode 100644 index 000000000000..c4d6867020c0 --- /dev/null +++ b/platforms/common/events.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include + +static orb_advert_t orb_event_pub = nullptr; +static pthread_mutex_t publish_event_mutex = PTHREAD_MUTEX_INITIALIZER; +static uint16_t event_sequence{events::initial_event_sequence}; + +namespace events +{ + +void send(EventType &event) +{ + event.timestamp = hrt_absolute_time(); + + // We need some synchronization here because: + // - modifying orb_event_pub + // - the update of event_sequence needs to be atomic + // - we need to ensure ordering of the sequence numbers: the sequence we set here + // has to be the one published next. + pthread_mutex_lock(&publish_event_mutex); + event.event_sequence = ++event_sequence; // Set the sequence here so we're able to detect uORB queue overflows + + if (orb_event_pub != nullptr) { + orb_publish(ORB_ID(event), orb_event_pub, &event); + + } else { + orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); + } + + pthread_mutex_unlock(&publish_event_mutex); +} + +} /* namespace events */ diff --git a/platforms/common/i2c.cpp b/platforms/common/i2c.cpp index 032224104d39..5d5d7e71e702 100644 --- a/platforms/common/i2c.cpp +++ b/platforms/common/i2c.cpp @@ -32,10 +32,11 @@ ****************************************************************************/ #include -#ifndef BOARD_DISABLE_I2C_SPI #include +#if defined(CONFIG_I2C) + #ifndef BOARD_OVERRIDE_I2C_BUS_EXTERNAL bool px4_i2c_bus_external(const px4_i2c_bus_t &bus) { @@ -85,4 +86,4 @@ bool I2CBusIterator::next() return false; } -#endif /* BOARD_DISABLE_I2C_SPI */ +#endif // CONFIG_I2C diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp index c738b068fbdd..91f4c6844fe0 100644 --- a/platforms/common/i2c_spi_buses.cpp +++ b/platforms/common/i2c_spi_buses.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2020, 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 @@ -32,7 +32,6 @@ ****************************************************************************/ #include -#ifndef BOARD_DISABLE_I2C_SPI #ifndef MODULE_NAME #define MODULE_NAME "SPI_I2C" @@ -49,17 +48,46 @@ static List i2c_spi_module_instances; ///< list of currently running instances static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER; + +I2CSPIDriverConfig::I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + const px4::wq_config_t &wq_config_) + : module_name(iterator.moduleName()), + devid_driver_index(iterator.devidDriverIndex()), + bus_option(iterator.configuredBusOption()), + bus_type(iterator.busType()), + bus(iterator.bus()), +#if defined(CONFIG_I2C) + i2c_address(cli.i2c_address), +#endif // CONFIG_I2C + bus_frequency(cli.bus_frequency), +#if defined(CONFIG_SPI) + drdy_gpio(iterator.DRDYGPIO()), + spi_mode(cli.spi_mode), + spi_devid(iterator.devid()), +#endif // CONFIG_SPI + bus_device_index(iterator.busDeviceIndex()), + rotation(cli.rotation), + quiet_start(cli.quiet_start), + keep_running(cli.keep_running), + custom1(cli.custom1), + custom2(cli.custom2), + custom_data(cli.custom_data), + wq_config(wq_config_) +{ + +} + const char *BusCLIArguments::parseDefaultArguments(int argc, char *argv[]) { - if (getopt(argc, argv, "") == EOF) { - return optarg(); + if (getOpt(argc, argv, "") == EOF) { + return optArg(); } // unexpected arguments return nullptr; } -int BusCLIArguments::getopt(int argc, char *argv[], const char *options) +int BusCLIArguments::getOpt(int argc, char *argv[], const char *options) { if (_options[0] == 0) { // need to initialize if (!validateConfiguration()) { @@ -68,6 +96,8 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) char *p = (char *)&_options; +#if defined(CONFIG_I2C) + if (_i2c_support) { *(p++) = 'X'; // external *(p++) = 'I'; // internal @@ -77,6 +107,9 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) } } +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + if (_spi_support) { *(p++) = 'S'; // external *(p++) = 's'; // internal @@ -84,6 +117,8 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) *(p++) = 'm'; *(p++) = ':'; // spi mode } +#endif // CONFIG_SPI + if (support_keep_running) { *(p++) = 'k'; } @@ -123,6 +158,8 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) while ((ch = px4_getopt(argc, argv, _options, &_optind, &_optarg)) != EOF) { switch (ch) { +#if defined(CONFIG_I2C) + case 'X': bus_option = I2CSPIBusOption::I2CExternal; break; @@ -138,6 +175,8 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) i2c_address = (int)strtol(_optarg, nullptr, 0); break; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) case 'S': bus_option = I2CSPIBusOption::SPIExternal; @@ -148,8 +187,9 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) break; case 'c': - chipselect_index = atoi(_optarg); + chipselect = atoi(_optarg); break; +#endif // CONFIG_SPI case 'b': requested_bus = atoi(_optarg); @@ -158,10 +198,12 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) case 'f': bus_frequency = 1000 * atoi(_optarg); break; +#if defined(CONFIG_SPI) case 'm': spi_mode = (spi_mode_e)atoi(_optarg); break; +#endif // CONFIG_SPI case 'q': quiet_start = true; @@ -191,12 +233,21 @@ int BusCLIArguments::getopt(int argc, char *argv[], const char *options) // apply defaults if not provided if (bus_frequency == 0) { +#if defined(CONFIG_I2C) + if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) { bus_frequency = default_i2c_frequency; - } else if (bus_option == I2CSPIBusOption::SPIExternal || bus_option == I2CSPIBusOption::SPIInternal) { + } + +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + + if (bus_option == I2CSPIBusOption::SPIExternal || bus_option == I2CSPIBusOption::SPIInternal) { bus_frequency = default_spi_frequency; } + +#endif // CONFIG_SPI } } @@ -207,28 +258,40 @@ bool BusCLIArguments::validateConfiguration() { bool success = true; +#if defined(CONFIG_I2C) + if (_i2c_support && default_i2c_frequency == -1) { PX4_ERR("Bug: driver %s does not set default_i2c_frequency", px4_get_taskname()); success = false; } +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) + if (_spi_support && default_spi_frequency == -1) { PX4_ERR("Bug: driver %s does not set default_spi_frequency", px4_get_taskname()); success = false; } +#endif // CONFIG_SPI return success; } BusInstanceIterator::BusInstanceIterator(const char *module_name, const BusCLIArguments &cli_arguments, uint16_t devid_driver_index) - : _module_name(module_name), _bus_option(cli_arguments.bus_option), _type(cli_arguments.type), + : _module_name(module_name), _bus_option(cli_arguments.bus_option), _devid_driver_index(devid_driver_index), +#if defined(CONFIG_I2C) _i2c_address(cli_arguments.i2c_address), +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) _spi_bus_iterator(spiFilter(cli_arguments.bus_option), - cli_arguments.bus_option == I2CSPIBusOption::SPIExternal ? cli_arguments.chipselect_index : devid_driver_index, + devid_driver_index, cli_arguments.chipselect, cli_arguments.requested_bus), +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) _i2c_bus_iterator(i2cFilter(cli_arguments.bus_option), cli_arguments.requested_bus), +#endif // CONFIG_I2C _current_instance(i2c_spi_module_instances.end()) { // We lock the module instance list as long as this object is alive, since we iterate over the list. @@ -256,7 +319,7 @@ bool BusInstanceIterator::next() while (_current_instance != i2c_spi_module_instances.end()) { if (strcmp((*_current_instance)->_module_name, _module_name) == 0 && - _type == (*_current_instance)->_type) { + _devid_driver_index == (*_current_instance)->_devid_driver_index) { return true; } @@ -265,20 +328,31 @@ bool BusInstanceIterator::next() return false; +#if defined(CONFIG_SPI) + } else if (busType() == BOARD_SPI_BUS) { if (_spi_bus_iterator.next()) { bus = _spi_bus_iterator.bus().bus; } - } else { +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) + + } else if (busType() == BOARD_I2C_BUS) { if (_i2c_bus_iterator.next()) { bus = _i2c_bus_iterator.bus().bus; } + +#endif // CONFIG_I2C } if (bus != -1) { // find matching runtime instance +#if defined(CONFIG_I2C) bool is_i2c = busType() == BOARD_I2C_BUS; +#else + bool is_i2c = false; +#endif for (_current_instance = i2c_spi_module_instances.begin(); _current_instance != i2c_spi_module_instances.end(); ++_current_instance) { @@ -287,8 +361,14 @@ bool BusInstanceIterator::next() } if (_bus_option == (*_current_instance)->_bus_option && bus == (*_current_instance)->_bus && - _type == (*_current_instance)->_type && - (!is_i2c || _i2c_address == (*_current_instance)->_i2c_address)) { + _devid_driver_index == (*_current_instance)->_devid_driver_index && + busDeviceIndex() == (*_current_instance)->_bus_device_index && + (!is_i2c +#if defined(CONFIG_I2C) + || _i2c_address == (*_current_instance)->_i2c_address +#endif // CONFIG_I2C + ) + ) { break; } } @@ -345,13 +425,19 @@ board_bus_types BusInstanceIterator::busType() const case I2CSPIBusOption::All: return BOARD_INVALID_BUS; +#if defined(CONFIG_I2C) + case I2CSPIBusOption::I2CInternal: case I2CSPIBusOption::I2CExternal: return BOARD_I2C_BUS; +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) case I2CSPIBusOption::SPIInternal: case I2CSPIBusOption::SPIExternal: return BOARD_SPI_BUS; +#endif // CONFIG_SPI } return BOARD_INVALID_BUS; @@ -359,69 +445,104 @@ board_bus_types BusInstanceIterator::busType() const int BusInstanceIterator::bus() const { - if (busType() == BOARD_INVALID_BUS) { - return -1; +#if defined(CONFIG_SPI) - } else if (busType() == BOARD_SPI_BUS) { + if (busType() == BOARD_SPI_BUS) { return _spi_bus_iterator.bus().bus; + } + +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) - } else { + if (busType() == BOARD_I2C_BUS) { return _i2c_bus_iterator.bus().bus; } + +#endif // CONFIG_I2C + + return -1; } uint32_t BusInstanceIterator::devid() const { - if (busType() == BOARD_INVALID_BUS) { - return 0; +#if defined(CONFIG_SPI) - } else if (busType() == BOARD_SPI_BUS) { + if (busType() == BOARD_SPI_BUS) { return _spi_bus_iterator.devid(); - - } else { - return 0; } + +#endif // CONFIG_SPI + + return 0; } +#if defined(CONFIG_SPI) spi_drdy_gpio_t BusInstanceIterator::DRDYGPIO() const { - if (busType() == BOARD_INVALID_BUS) { - return 0; - - } else if (busType() == BOARD_SPI_BUS) { + if (busType() == BOARD_SPI_BUS) { return _spi_bus_iterator.DRDYGPIO(); - - } else { - return 0; } + + return 0; } +#endif // CONFIG_SPI bool BusInstanceIterator::external() const { - if (busType() == BOARD_INVALID_BUS) { - return false; +#if defined(CONFIG_SPI) - } else if (busType() == BOARD_SPI_BUS) { + if (busType() == BOARD_SPI_BUS) { return _spi_bus_iterator.external(); + } + +#endif // CONFIG_SPI + +#if defined(CONFIG_I2C) - } else { + if (busType() == BOARD_I2C_BUS) { return _i2c_bus_iterator.external(); } + +#endif // CONFIG_I2C + + return false; } int BusInstanceIterator::externalBusIndex() const { - if (busType() == BOARD_INVALID_BUS) { - return 0; +#if defined(CONFIG_SPI) - } else if (busType() == BOARD_SPI_BUS) { + if (busType() == BOARD_SPI_BUS) { return _spi_bus_iterator.externalBusIndex(); + } + +#endif // CONFIG_SPI - } else { +#if defined(CONFIG_I2C) + + if (busType() == BOARD_I2C_BUS) { return _i2c_bus_iterator.externalBusIndex(); } + +#endif // CONFIG_I2C + + return 0; } +int BusInstanceIterator::busDeviceIndex() const +{ +#if defined(CONFIG_SPI) + + if (busType() == BOARD_SPI_BUS) { + return _spi_bus_iterator.busDeviceIndex(); + } + +#endif // CONFIG_SPI + + return -1; +} + +#if defined(CONFIG_I2C) I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_option) { switch (bus_option) { @@ -436,7 +557,9 @@ I2CBusIterator::FilterType BusInstanceIterator::i2cFilter(I2CSPIBusOption bus_op return I2CBusIterator::FilterType::All; } +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_option) { switch (bus_option) { @@ -449,10 +572,10 @@ SPIBusIterator::FilterType BusInstanceIterator::spiFilter(I2CSPIBusOption bus_op return SPIBusIterator::FilterType::InternalBus; } +#endif // CONFIG_SPI struct I2CSPIDriverInitializing { - const BusCLIArguments &cli; - const BusInstanceIterator &iterator; + const I2CSPIDriverConfig &config; I2CSPIDriverBase::instantiate_method instantiate; int runtime_instance; I2CSPIDriverBase *instance{nullptr}; @@ -461,7 +584,7 @@ struct I2CSPIDriverInitializing { static void initializer_trampoline(void *argument) { I2CSPIDriverInitializing *data = (I2CSPIDriverInitializing *)argument; - data->instance = data->instantiate(data->cli, data->iterator, data->runtime_instance); + data->instance = data->instantiate(data->config, data->runtime_instance); } int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator, @@ -486,17 +609,26 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat device_id.devid_s.bus = iterator.bus(); switch (iterator.busType()) { +#if defined(CONFIG_I2C) + case BOARD_I2C_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_I2C; break; +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) case BOARD_SPI_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_SPI; break; +#endif // CONFIG_SPI case BOARD_INVALID_BUS: device_id.devid_s.bus_type = device::Device::DeviceBusType_UNKNOWN; break; } + + const px4::wq_config_t &wq_config = px4::device_bus_to_wq(device_id.devid); + I2CSPIDriverConfig driver_config{cli, iterator, wq_config}; const int runtime_instance = iterator.runningInstancesCount(); - I2CSPIDriverInitializing initializer_data{cli, iterator, instantiate, runtime_instance}; + I2CSPIDriverInitializing initializer_data{driver_config, instantiate, runtime_instance}; // initialize the object and bus on the work queue thread - this will also probe for the device - px4::WorkItemSingleShot initializer(px4::device_bus_to_wq(device_id.devid), initializer_trampoline, &initializer_data); + px4::WorkItemSingleShot initializer(wq_config, initializer_trampoline, &initializer_data); initializer.ScheduleNow(); initializer.wait(); I2CSPIDriverBase *instance = initializer_data.instance; @@ -506,33 +638,57 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat continue; } +#if defined(CONFIG_I2C) + if (cli.i2c_address != 0 && instance->_i2c_address == 0) { PX4_ERR("Bug: driver %s does not pass the I2C address to I2CSPIDriverBase", instance->ItemName()); } +#endif // CONFIG_I2C + iterator.addInstance(instance); started = true; // print some info that we are running switch (iterator.busType()) { +#if defined(CONFIG_I2C) + case BOARD_I2C_BUS: - PX4_INFO_RAW("%s #%i on I2C bus %d%s\n", instance->ItemName(), runtime_instance, iterator.bus(), - iterator.external() ? " (external)" : ""); + PX4_INFO_RAW("%s #%i on I2C bus %d", instance->ItemName(), runtime_instance, iterator.bus()); + + if (iterator.external()) { + PX4_INFO_RAW(" (external)"); + } + + if (cli.i2c_address != 0) { + PX4_INFO_RAW(" address 0x%X", cli.i2c_address); + } + + if (cli.rotation != 0) { + PX4_INFO_RAW(" rotation %d", cli.rotation); + } + + PX4_INFO_RAW("\n"); break; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) case BOARD_SPI_BUS: - PX4_INFO_RAW("%s #%i on SPI bus %d (devid=0x%x)", - instance->ItemName(), runtime_instance, iterator.bus(), PX4_SPI_DEV_ID(iterator.devid())); + PX4_INFO_RAW("%s #%i on SPI bus %d", instance->ItemName(), runtime_instance, iterator.bus()); if (iterator.external()) { - PX4_INFO_RAW(" (external, equal to '-b %i')\n", iterator.externalBusIndex()); + PX4_INFO_RAW(" (external, equal to '-b %i')", iterator.externalBusIndex()); + } - } else { - PX4_INFO_RAW("\n"); + if (cli.rotation != 0) { + PX4_INFO_RAW(" rotation %d", cli.rotation); } + PX4_INFO_RAW("\n"); + break; +#endif // CONFIG_SPI case BOARD_INVALID_BUS: break; @@ -540,13 +696,27 @@ int I2CSPIDriverBase::module_start(const BusCLIArguments &cli, BusInstanceIterat } if (!started && !cli.quiet_start) { - PX4_WARN("%s: no instance started (no device on bus?)", px4_get_taskname()); + static constexpr char no_instance_started[] {"no instance started (no device on bus?)"}; + + if (iterator.external()) { + PX4_WARN("%s: %s", px4_get_taskname(), no_instance_started); + + } else { + PX4_ERR("%s: %s", px4_get_taskname(), no_instance_started); + } + +#if defined(CONFIG_I2C) + + if (iterator.busType() == BOARD_I2C_BUS && cli.i2c_address == 0) { + PX4_ERR("%s: driver does not set i2c address", px4_get_taskname()); + } + +#endif // CONFIG_I2C } return started ? 0 : -1; } - int I2CSPIDriverBase::module_stop(BusInstanceIterator &iterator) { bool is_running = false; @@ -624,8 +794,23 @@ int I2CSPIDriverBase::module_custom_method(const BusCLIArguments &cli, BusInstan void I2CSPIDriverBase::print_status() { - bool is_i2c_bus = _bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal; - PX4_INFO("Running on %s Bus %i", is_i2c_bus ? "I2C" : "SPI", _bus); +#if defined(CONFIG_I2C) + + if (_bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal) { + PX4_INFO("Running on I2C Bus %i, Address 0x%02X", _bus, get_i2c_address()); + return; + } + +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) + + if (_bus_option == I2CSPIBusOption::SPIExternal || _bus_option == I2CSPIBusOption::SPIInternal) { + PX4_INFO("Running on SPI Bus %i", _bus); + return; + } + +#endif // CONFIG_SPI } void I2CSPIDriverBase::request_stop_and_wait() @@ -643,5 +828,3 @@ void I2CSPIDriverBase::request_stop_and_wait() PX4_ERR("Module did not respond to stop request"); } } - -#endif /* BOARD_DISABLE_I2C_SPI */ diff --git a/platforms/common/include/libevents_definitions.h b/platforms/common/include/libevents_definitions.h new file mode 100644 index 000000000000..4706a20693ba --- /dev/null +++ b/platforms/common/include/libevents_definitions.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/* + * This header defines the events::EventType type. + */ + +#pragma once + +#include + +namespace events +{ +using EventType = event_s; +} // namespace events + + + diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 3f8c941b2523..b2d828efdb9e 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -58,6 +58,10 @@ #include #include +#if defined(__PX4_NUTTX) +# include +#endif // __PX4_NUTTX + namespace px4 { @@ -66,11 +70,11 @@ class atomic { public: -#ifdef __PX4_NUTTX +#if defined(__PX4_POSIX) // Ensure that all operations are lock-free, so that 'atomic' can be used from // IRQ handlers. This might not be required everywhere though. static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); -#endif +#endif // __PX4_POSIX atomic() = default; explicit atomic(T value) : _value(value) {} @@ -80,11 +84,19 @@ class atomic */ inline T load() const { -#ifdef __PX4_QURT - return _value; -#else - return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); -#endif +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); + } } /** @@ -92,11 +104,18 @@ class atomic */ inline void store(T value) { -#ifdef __PX4_QURT - _value = value; -#else - __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); -#endif +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + _value = value; + leave_critical_section(flags); + + } else +#endif // __PX4_NUTTX + { + __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); + } } /** @@ -105,12 +124,19 @@ class atomic */ inline T fetch_add(T num) { -#ifdef __PX4_QURT - // TODO: fix - return _value++; -#else - return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); -#endif +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value++; + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -119,7 +145,19 @@ class atomic */ inline T fetch_sub(T num) { - return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value--; + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -128,7 +166,20 @@ class atomic */ inline T fetch_and(T num) { - return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value &= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -137,7 +188,20 @@ class atomic */ inline T fetch_xor(T num) { - return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value ^= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -146,7 +210,20 @@ class atomic */ inline T fetch_or(T num) { - return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value |= num; + leave_critical_section(flags); + return val; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -155,7 +232,20 @@ class atomic */ inline T fetch_nand(T num) { - return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value = ~(_value & num); + leave_critical_section(flags); + return ret; + + } else +#endif // __PX4_NUTTX + { + return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); + } } /** @@ -168,17 +258,31 @@ class atomic */ inline bool compare_exchange(T *expected, T desired) { - return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); +#if defined(__PX4_NUTTX) + + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + + if (_value == *expected) { + _value = desired; + leave_critical_section(flags); + return true; + + } else { + *expected = _value; + leave_critical_section(flags); + return false; + } + + } else +#endif // __PX4_NUTTX + { + return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); + } } private: -#ifdef __PX4_QURT - // It seems that __atomic_store and __atomic_load are not supported on Qurt, - // so the best that we can do is to use volatile. - volatile T _value{}; -#else T _value {}; -#endif }; using atomic_int = atomic; diff --git a/platforms/common/include/px4_platform_common/atomic_bitset.h b/platforms/common/include/px4_platform_common/atomic_bitset.h index 1cd97eef4916..1f5bef969a76 100644 --- a/platforms/common/include/px4_platform_common/atomic_bitset.h +++ b/platforms/common/include/px4_platform_common/atomic_bitset.h @@ -80,6 +80,14 @@ class AtomicBitset } } + void reset() + { + // set bits to false + for (auto &d : _data) { + d.store(0); + } + } + private: static constexpr uint8_t BITS_PER_ELEMENT = 32; static constexpr size_t ARRAY_SIZE = ((N % BITS_PER_ELEMENT) == 0) ? (N / BITS_PER_ELEMENT) : diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 53a3eaa57157..4acfa6554967 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -145,7 +145,10 @@ # define BOARD_BATT_I_LIST {ADC_BATTERY_CURRENT_CHANNEL} # define BOARD_BRICK_VALID_LIST {BOARD_ADC_BRICK_VALID} #elif BOARD_NUMBER_BRICKS == 2 -# if !defined(BOARD_NUMBER_DIGITAL_BRICKS) +# if defined(BOARD_NUMBER_DIGITAL_BRICKS) +# define BOARD_BATT_V_LIST {-1, -1} +# define BOARD_BATT_I_LIST {-1, -1} +# else # define BOARD_BATT_V_LIST {ADC_BATTERY1_VOLTAGE_CHANNEL, ADC_BATTERY2_VOLTAGE_CHANNEL} # define BOARD_BATT_I_LIST {ADC_BATTERY1_CURRENT_CHANNEL, ADC_BATTERY2_CURRENT_CHANNEL} # endif @@ -240,13 +243,6 @@ # define BOARD_EEPROM_WP_CTRL(on_true) #endif -/* - * Defined when a board has capture and uses channels. - */ -#if defined(DIRECT_INPUT_TIMER_CHANNELS) && DIRECT_INPUT_TIMER_CHANNELS > 0 -#define BOARD_HAS_CAPTURE 1 -#endif - /* * Defined when a supports version and type API. */ @@ -262,6 +258,7 @@ #if defined(BOARD_HAS_HW_VERSIONING) # define BOARD_HAS_VERSIONING 1 +# define HW_VER_REV(v,r) ((uint32_t)((v) & 0xff) << 8) | ((uint32_t)(r) & 0xff) #endif /* Default LED logical to color mapping */ @@ -505,7 +502,14 @@ static inline bool board_rc_invert_input(const char *device, bool invert) { retu * ************************************************************************************/ -#if defined(GPIO_OTGFS_VBUS) +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +inline static int board_read_VBUS_state(void) +{ + platformiocvbusstate_t state = {false}; + boardctl(PLATFORMIOCVBUSSTATE, (uintptr_t)&state); + return state.ret; +} +#elif defined(GPIO_OTGFS_VBUS) # define board_read_VBUS_state() (px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1) #else int board_read_VBUS_state(void); @@ -559,9 +563,9 @@ __EXPORT void board_on_reset(int status); * ****************************************************************************/ -#ifdef CONFIG_BOARDCTL_POWEROFF +#if defined(BOARD_HAS_POWER_CONTROL) int board_power_off(int status); -#endif +#endif // BOARD_HAS_POWER_CONTROL /**************************************************************************** * Name: board_reset @@ -1021,8 +1025,12 @@ int board_register_power_state_notification_cb(power_button_state_notification_t enum board_bus_types { BOARD_INVALID_BUS = 0, +#if defined(CONFIG_SPI) BOARD_SPI_BUS = 1, +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) BOARD_I2C_BUS = 2 +#endif // CONFIG_I2C }; #if defined(BOARD_HAS_BUS_MANIFEST) diff --git a/platforms/common/include/px4_platform_common/crypto.h b/platforms/common/include/px4_platform_common/crypto.h new file mode 100644 index 000000000000..554b6d4891df --- /dev/null +++ b/platforms/common/include/px4_platform_common/crypto.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +#ifdef PX4_CRYPTO + +#include +#include + +#include +#include +#include +#include "crypto_backend_definitions.h" + +/* + * Crypto API interface class + */ + +class PX4Crypto +{ +public: + /* + * Constructor & destructor + */ + + PX4Crypto(); + ~PX4Crypto(); + + /* + * Class function for crypto api initialization, called only once at + * boot + */ + + static void px4_crypto_init(void); + + /* + * Open crypto API for a specific algorithm + * algorithm: The crypto algorithm to be used + * returns true on success, false on failure + */ + + bool open(px4_crypto_algorithm_t algorithm); + + /* + * Close the crypto API. Optional, it is also closed by destructing the + * interface object + */ + + void close(); + + /* + * Keystore access functions + */ + + /* + * Generate a single random key for symmetric-key encryption + * + * idx: the index in keystore where the key will be stored + * persistent: whether the key need to be stored persistently + * returns true on success, false on failure + */ + + bool generate_key(uint8_t idx, + bool persistent); + + /* + * Generate a key pair for asymmetric-key encryption + * + * algorithm: the key type + * key_size: size of the key in bytes + * private_idx: the private key will be stored in this index in the keystore + * public_idx: the public key will be stored in this index in the keystore + * persistent: whether the keys need to be stored persistently + * returns true on success, false on failure + */ + + bool generate_keypair(size_t key_size, + uint8_t private_idx, + uint8_t public_idx, + bool persistent); + + + /* + * Re-create or set nonce. + * + * A nonce or intialization vector value for the selected algortithm is + * automatically generated when the crypto session is opened. If needed, the + * nonce can be set by this function. + * If this is called with NULL pointer, a new nonce is automatically random + * generated + */ + + bool renew_nonce(const uint8_t *nonce, size_t nonce_size); + + + /* + * Get current crypto session nonce + * + * This function returns the current nonce for the session + * If the "nonce" is NULL, only nonse legth will be provided + * nonce: pointer to the buffer where the nonce will be written + * nonce_len: length of the current nonce vector for the session + * returns true on success, false on failure + */ + + bool get_nonce(uint8_t *nonce, + size_t *nonce_len); + + /* + * Store a key into keystore + * + * encryption_idx: The key index in keystore to be used in decrypting and + * authenticating the key before storing + * key: The pointer to the key + * key_idx: Index where the key will be stored in keystore + */ + + + bool set_key(uint8_t encryption_idx, + const uint8_t *key, + uint8_t key_idx); + + /* + * Get a key from keystore. Key can be encrypted + * + * key_idx: Index of the requested key in the keystore + * key: The provided buffer to the key. If NULL, the function only provides + * the length of the key. + * key_len: input: the size of the provided "key" buffer. + output: the actual size of the key + * encryption_key_idx: The key index in keystore to be used for encrypting + * returns true on success, false on failure + * + */ + + bool get_encrypted_key(uint8_t key_idx, + uint8_t *key, + size_t *key_len, + uint8_t encryption_key_idx); + + /* + * PX4 Crypto API functions + */ + + /* + * Verify signature + * + * key_index: public key index in keystore + * signature: pointer to the signature + * message: pointer to the data to be verified + * message_size: size of the message in bytes + */ + + bool signature_check(uint8_t key_index, + const uint8_t *signature, + const uint8_t *message, + size_t message_size); + + + /* + * Encrypt data. This always supports encryption in-place + * + * key_index: key index in keystore + * message: pointer to the message + * message_size: size of the message in bytes + * cipher: pointer to a buffer for encrypted data + * cipher_size: size of the buffer reserved for cipher and actual cipher length + * after the encryption + * returns true on success, false on failure + */ + + bool encrypt_data(uint8_t key_index, + const uint8_t *message, + size_t message_size, + uint8_t *cipher, + size_t *cipher_size); + + size_t get_min_blocksize(uint8_t key_idx); + +private: + crypto_session_handle_t _crypto_handle; + static px4_sem_t _lock; + static bool _initialized; + static void lock() { do {} while (px4_sem_wait(&PX4Crypto::_lock) != 0); } + static void unlock() { px4_sem_post(&PX4Crypto::_lock); } +}; + +#endif diff --git a/platforms/common/include/px4_platform_common/crypto_algorithms.h b/platforms/common/include/px4_platform_common/crypto_algorithms.h new file mode 100644 index 000000000000..32a4a44f5f48 --- /dev/null +++ b/platforms/common/include/px4_platform_common/crypto_algorithms.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +/* List of all the supported crypto algorithms in PX4 */ + +typedef enum { + CRYPTO_NONE = 0, + CRYPTO_ED25519 = 1, + CRYPTO_XCHACHA20 = 2, + CRYPTO_AES = 3, + CRYPTO_RSA_OAEP = 4, +} px4_crypto_algorithm_t; diff --git a/platforms/common/include/px4_platform_common/crypto_backend.h b/platforms/common/include/px4_platform_common/crypto_backend.h new file mode 100644 index 000000000000..1b1df6e87bb1 --- /dev/null +++ b/platforms/common/include/px4_platform_common/crypto_backend.h @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +#if defined(__cplusplus) +extern "C" { +#endif + +#include +#include +#include +#include "crypto_backend_definitions.h" + +/* + * Keystore access functions + */ + +void keystore_init(void); + +/* + * Open a session for accessing security keys + */ + +keystore_session_handle_t keystore_open(void); + +/* + * Close the keystore session + * handle: handle to the session to be closed + */ + +void keystore_close(keystore_session_handle_t *handle); + +/* + * Get a key from keystore + * handle: a handle to an open keystore + * idx: key index in keystore + * key_buf: output buffer for the key + * key_buf_size: size of the provided output buffer + * returns the size of the key in keystore + */ +size_t keystore_get_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *key_buf, size_t key_buf_size); + +/* + * Store a key persistently into the keystore + * handle: a handle to an open keystore + * idx: key index in keystore + * key: pointer to the key + * key_size: size of the key + */ +bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *key, size_t key_size); + + +/* + * Architecture specific PX4 Crypto API functions + */ + +/* + * Initialize hw level crypto + * This has to be called before any other crypto operations + */ + +void crypto_init(void); + +/* + * Open a session for performing crypto functions + * algorithm: The crypto algorithm to be used in this session + */ + +crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm); + +/* + * Close the session for performing crypto operations + * handle: handle to the session to be closed + */ + +void crypto_close(crypto_session_handle_t *handle); + +/* + * Generate a key + * handle: Open handle for the crypto session. The key will be generated for + * the crypto algorithm used by this session + * idx: The key index, by which the key can be used + * persistent: if set to "true", the key will be stored into the keystore + */ +bool crypto_generate_key(crypto_session_handle_t handle, + uint8_t idx, + bool persistent); + +/* + * Get a key from keystore, possibly encrypted + * + * handle: an open crypto context; the returned key will be encrypted + * according to this context + * key_idx: Index of the requested key in the keystore + * key: The provided buffer to the key + * max_len: the length of the provided key buffer. Returns the actual key size + * encryption_key_idx: The key index in keystore to be used for encrypting + * returns true on success, false on failure + */ +bool crypto_get_encrypted_key(crypto_session_handle_t handle, + uint8_t key_idx, + uint8_t *key, + size_t *max_len, + uint8_t encryption_key_idx); + +/* + * Get the generated nonce value + * + * handle: an open crypto context; the returned nonce is the one associsated + * with this context/algorithm + * nonce: The provided buffer to the key. If NULL, only length is returned + * nonce_len: the length of the nonce value + * encryption_key_idx: The key index in keystore to be used for encrypting + * returns true on success, false on failure + */ +bool crypto_get_nonce(crypto_session_handle_t handle, + uint8_t *nonce, + size_t *nonce_len); + +/* + * Perform signature check using an open session to crypto + * handle: session handle, returned by open + * key_index: index to the key used for signature check + * message: pointer to the data to be checked + * message_size: size of the data + */ + +bool crypto_signature_check(crypto_session_handle_t handle, + uint8_t key_index, + const uint8_t *signature, + const uint8_t *message, + size_t message_size); + +bool crypto_encrypt_data(crypto_session_handle_t handle, + uint8_t key_index, + const uint8_t *message, + size_t message_size, + uint8_t *cipher, + size_t *cipher_size); + +/* + * Returns a minimum data block size on which the crypto operations can be + * performed. Performing encryption on sizes which are not multiple of this + * are valid, but the output will be padded to the multiple of this value + * Input for decryption must be multiple of this value. + * handle: session handle, returned by open + * key_idx: key to be used for encryption/decryption + * returs the block size + */ + +size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx); + +#if defined(__cplusplus) +} // extern "C" +#endif diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 7038130b2f69..6c325fa803a4 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -50,8 +50,10 @@ /* Define PX4_ISFINITE */ #ifdef __cplusplus -constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); } -constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } +static inline constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); } +static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } +#else +#define PX4_ISFINITE(x) __builtin_isfinite(x) #endif /* __cplusplus */ #if defined(__PX4_NUTTX) @@ -89,33 +91,14 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } #define USEC_PER_TICK (1000000/PX4_TICKS_PER_SEC) #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) -#ifdef __PX4_QURT - -// QURT specific -# include "dspal_math.h" -# define PX4_ROOTFSDIR "." -# define PX4_TICKS_PER_SEC 1000L - -#else // __PX4_QURT - -// All POSIX except QURT. - __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -# if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) -# define PX4_ROOTFSDIR "/home/linaro" -# else -# define PX4_ROOTFSDIR "." -# endif - -#endif // __PX4_QURT +#define PX4_ROOTFSDIR "." #define PX4_STORAGEDIR PX4_ROOTFSDIR -#endif // __PX4_POSIX -#if defined(__PX4_POSIX) /**************************************************************************** * Defines for POSIX and ROS ****************************************************************************/ diff --git a/platforms/common/include/px4_platform_common/events.h b/platforms/common/include/px4_platform_common/events.h new file mode 100644 index 000000000000..a5aed0b732c8 --- /dev/null +++ b/platforms/common/include/px4_platform_common/events.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// Console event output can be enabled if needed. +// Note: this will only print custom (PX4) events, not from common. +// Helpful for debugging and to ensure the system is not spammed with events. +// It does not print arguments. +#if 0 +#include +#define CONSOLE_PRINT_EVENT(log_level, id, str) PX4_INFO_RAW("Event 0x%08x: %s\n", id, str) +#else +#define CONSOLE_PRINT_EVENT(log_level, id, str) +#endif + +#include +#include + +#include + +namespace events +{ + +namespace util +{ + +// source: https://gist.github.com/ruby0x1/81308642d0325fd386237cfa3b44785c +constexpr uint32_t val_32_const = 0x811c9dc5; +constexpr uint32_t prime_32_const = 0x1000193; +constexpr uint64_t val_64_const = 0xcbf29ce484222325; +constexpr uint64_t prime_64_const = 0x100000001b3; +inline constexpr uint32_t hash_32_fnv1a_const(const char *const str, const uint32_t value = val_32_const) noexcept +{ + return (str[0] == '\0') ? value : hash_32_fnv1a_const(&str[1], (value ^ uint32_t(str[0])) * prime_32_const); +} + +template +inline constexpr void fillEventArguments(uint8_t *buf, T arg) +{ + // This assumes we're on little-endian + memcpy(buf, &arg, sizeof(T)); +} + +template +inline constexpr void fillEventArguments(uint8_t *buf, T arg, Args... args) +{ + fillEventArguments(buf, arg); + fillEventArguments(buf + sizeof(T), args...); +} + +constexpr unsigned sizeofArguments() { return 0; } + +template +constexpr unsigned sizeofArguments(const T &t, const Args &... args) +{ + return sizeof(T) + sizeofArguments(args...); +} + +} // namespace util + + +/** + * publish/send an event + */ +void send(EventType &event); + +/** + * Generate event ID from an event name + */ +template +constexpr uint32_t ID(const char (&name)[N]) +{ + // Note: the generated ID must match with the python generator under Tools/px4events + uint32_t component_id = 1u << 24; // autopilot component + return (0xffffff & util::hash_32_fnv1a_const(name)) | component_id; +} + +template +inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args) +{ + EventType e{}; + e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; + e.id = id; + static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments"); + util::fillEventArguments((uint8_t *)e.arguments, args...); + CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message); + send(e); +} + +inline void send(uint32_t id, const LogLevels &log_levels, const char *message) +{ + EventType e{}; + e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; + e.id = id; + CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message); + send(e); +} + +static constexpr uint16_t initial_event_sequence = 65535 - 10; // initialize with a high number so it wraps soon + +} // namespace events diff --git a/platforms/common/include/px4_platform_common/i2c.h b/platforms/common/include/px4_platform_common/i2c.h index 0390687df038..16c274860f81 100644 --- a/platforms/common/include/px4_platform_common/i2c.h +++ b/platforms/common/include/px4_platform_common/i2c.h @@ -35,6 +35,8 @@ #include +#if defined(CONFIG_I2C) + #define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES struct px4_i2c_bus_t { @@ -99,3 +101,5 @@ class I2CBusIterator int _index{-1}; int _external_bus_counter{0}; }; + +#endif // CONFIG_I2C diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index b3a6f7c72e23..85c8c634847f 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -33,8 +33,7 @@ #pragma once -#include "i2c.h" -#include "spi.h" +#include #include @@ -43,15 +42,64 @@ #include #include #include -#include -#include + +#if defined(CONFIG_I2C) +# include "i2c.h" +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) +# include "spi.h" +#endif // CONFIG_SPI + +#if defined(CONFIG_SPI) +# include +#endif // CONFIG_SPI enum class I2CSPIBusOption : uint8_t { All = 0, ///< select all runnning instances +#if defined(CONFIG_I2C) I2CInternal, I2CExternal, +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) SPIInternal, SPIExternal, +#endif // CONFIG_SPI +}; + +class BusCLIArguments; +class BusInstanceIterator; + +struct I2CSPIDriverConfig { + I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + const px4::wq_config_t &wq_config_); + + const char *module_name; + uint16_t devid_driver_index; + I2CSPIBusOption bus_option; + board_bus_types bus_type; + int bus; +#if defined(CONFIG_I2C) + uint8_t i2c_address; +#endif // CONFIG_I2C + int bus_frequency; +#if defined(CONFIG_SPI) + spi_drdy_gpio_t drdy_gpio; + spi_mode_e spi_mode; + uint32_t spi_devid; +#endif // CONFIG_SPI + int bus_device_index; + + Rotation rotation; + + bool quiet_start; + bool keep_running; + + int custom1; + int custom2; + void *custom_data; + + const px4::wq_config_t &wq_config; }; /** @@ -62,10 +110,18 @@ class I2CSPIInstance : public ListNode { public: virtual ~I2CSPIInstance() = default; - +#if defined(CONFIG_I2C) + virtual int8_t get_i2c_address() {return _i2c_address;} +#endif // CONFIG_I2C private: - I2CSPIInstance(const char *module_name, I2CSPIBusOption bus_option, int bus, uint8_t i2c_address, uint16_t type) - : _module_name(module_name), _bus_option(bus_option), _bus(bus), _type(type), _i2c_address(i2c_address) {} + I2CSPIInstance(const I2CSPIDriverConfig &config) + : _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus), + _devid_driver_index(config.devid_driver_index), _bus_device_index(config.bus_device_index) +#if defined(CONFIG_I2C) + , _i2c_address(config.i2c_address) +#endif // CONFIG_I2C + {} + friend class BusInstanceIterator; friend class I2CSPIDriverBase; @@ -73,15 +129,30 @@ class I2CSPIInstance : public ListNode const char *_module_name; const I2CSPIBusOption _bus_option; const int _bus; - const int16_t _type; ///< device type (driver-specific) + const uint16_t _devid_driver_index; + const int8_t _bus_device_index; +#if defined(CONFIG_I2C) const int8_t _i2c_address; ///< I2C address (optional) +#endif // CONFIG_I2C }; class BusCLIArguments { public: BusCLIArguments(bool i2c_support, bool spi_support) - : _i2c_support(i2c_support), _spi_support(spi_support) {} +#if defined(CONFIG_I2C) || defined(CONFIG_SPI) + : +#endif // CONFIG_I2C || CONFIG_SPI +#if defined(CONFIG_I2C) + _i2c_support(i2c_support) +#endif // CONFIG_I2C +#if defined(CONFIG_I2C) && defined(CONFIG_SPI) + , +#endif // CONFIG_I2C && CONFIG_SPI +#if defined(CONFIG_SPI) + _spi_support(spi_support) +#endif // CONFIG_SPI + {} /** * Parse CLI arguments (for drivers that don't need any custom arguments, otherwise getopt() should be used) @@ -92,35 +163,41 @@ class BusCLIArguments /** * Like px4_getopt(), but adds and handles i2c/spi driver-specific arguments */ - int getopt(int argc, char *argv[], const char *options); + int getOpt(int argc, char *argv[], const char *options); /** * returns the current optional argument (for options like 'T:'), or the command (e.g. "start") * @return nullptr or argument/command */ - const char *optarg() const { return _optarg; } + const char *optArg() const { return _optarg; } I2CSPIBusOption bus_option{I2CSPIBusOption::All}; - uint16_t type{0}; ///< device type (driver-specific) int requested_bus{-1}; - int chipselect_index{1}; - Rotation rotation{ROTATION_NONE}; int bus_frequency{0}; +#if defined(CONFIG_SPI) + int chipselect {-1}; spi_mode_e spi_mode{SPIDEV_MODE3}; - uint8_t i2c_address{0}; ///< optional I2C address: a driver can set this to allow configuring the I2C address - bool quiet_start{false}; ///< do not print a message when startup fails +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) + uint8_t i2c_address {0}; ///< I2C address (a driver must set the default address) +#endif // CONFIG_I2C + bool quiet_start {false}; ///< do not print a message when startup fails bool keep_running{false}; ///< keep driver running even if no device is detected on startup - uint8_t orientation{0}; ///< distance_sensor_s::ROTATION_* + Rotation rotation{ROTATION_NONE}; ///< sensor rotation (MAV_SENSOR_ROTATION_* or distance_sensor_s::ROTATION_*) int custom1{0}; ///< driver-specific custom argument int custom2{0}; ///< driver-specific custom argument void *custom_data{nullptr}; ///< driver-specific custom argument // driver defaults, if not specified via CLI - int default_spi_frequency{-1}; ///< default spi bus frequency (driver needs to set this) [Hz] - int default_i2c_frequency{-1}; ///< default i2c bus frequency (driver needs to set this) [Hz] +#if defined(CONFIG_SPI) + int default_spi_frequency {-1}; ///< default spi bus frequency (driver needs to set this) [Hz] +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) + int default_i2c_frequency {-1}; ///< default i2c bus frequency (driver needs to set this) [Hz] +#endif // CONFIG_I2C bool support_keep_running{false}; ///< true if keep_running (see above) is supported @@ -130,8 +207,12 @@ class BusCLIArguments char _options[32] {}; int _optind{1}; const char *_optarg{nullptr}; +#if defined(CONFIG_I2C) const bool _i2c_support; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) const bool _spi_support; +#endif // CONFIG_SPI }; /** @@ -155,21 +236,40 @@ class BusInstanceIterator board_bus_types busType() const; int bus() const; uint32_t devid() const; + +#if defined(CONFIG_SPI) spi_drdy_gpio_t DRDYGPIO() const; +#endif // CONFIG_SPI + bool external() const; int externalBusIndex() const; + int busDeviceIndex() const; void addInstance(I2CSPIInstance *instance); +#if defined(CONFIG_I2C) static I2CBusIterator::FilterType i2cFilter(I2CSPIBusOption bus_option); +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) static SPIBusIterator::FilterType spiFilter(I2CSPIBusOption bus_option); +#endif // CONFIG_SPI + + const char *moduleName() const { return _module_name; } + uint16_t devidDriverIndex() const { return _devid_driver_index; } + private: const char *_module_name; const I2CSPIBusOption _bus_option; - const uint16_t _type; + const uint16_t _devid_driver_index; +#if defined(CONFIG_I2C) const uint8_t _i2c_address; +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) SPIBusIterator _spi_bus_iterator; +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) I2CBusIterator _i2c_bus_iterator; +#endif // CONFIG_I2C List::Iterator _current_instance; }; @@ -180,18 +280,16 @@ class BusInstanceIterator class I2CSPIDriverBase : public px4::ScheduledWorkItem, public I2CSPIInstance { public: - I2CSPIDriverBase(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, - uint8_t i2c_address, uint16_t type) - : ScheduledWorkItem(module_name, config), - I2CSPIInstance(module_name, bus_option, bus, i2c_address, type) {} + I2CSPIDriverBase(const I2CSPIDriverConfig &config) + : ScheduledWorkItem(config.module_name, config.wq_config), + I2CSPIInstance(config) {} static int module_stop(BusInstanceIterator &iterator); static int module_status(BusInstanceIterator &iterator); static int module_custom_method(const BusCLIArguments &cli, BusInstanceIterator &iterator, bool run_on_work_queue = true); - using instantiate_method = I2CSPIDriverBase * (*)(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + using instantiate_method = I2CSPIDriverBase * (*)(const I2CSPIDriverConfig &config, int runtime_instance); protected: virtual ~I2CSPIDriverBase() = default; @@ -230,13 +328,12 @@ class I2CSPIDriver : public I2CSPIDriverBase public: static int module_start(const BusCLIArguments &cli, BusInstanceIterator &iterator) { - return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, &T::instantiate); + return I2CSPIDriverBase::module_start(cli, iterator, &T::print_usage, InstantiateHelper::m); } protected: - I2CSPIDriver(const char *module_name, const px4::wq_config_t &config, I2CSPIBusOption bus_option, int bus, - uint8_t i2c_address = 0, uint16_t type = 0) - : I2CSPIDriverBase(module_name, config, bus_option, bus, i2c_address, type) {} + I2CSPIDriver(const I2CSPIDriverConfig &config) + : I2CSPIDriverBase(config) {} virtual ~I2CSPIDriver() = default; @@ -251,4 +348,33 @@ class I2CSPIDriver : public I2CSPIDriverBase } // *INDENT-ON* private: + + // SFINAE to use R::instantiate if it exists, and R::instantiate_default otherwise + template + class InstantiateHelper + { + template + static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; } + template + static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; } + public: + static constexpr I2CSPIDriverBase::instantiate_method m = get(0); + }; + + static I2CSPIDriverBase *instantiate_default(const I2CSPIDriverConfig &config, int runtime_instance) + { + T *instance = new T(config); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; + } }; diff --git a/platforms/common/include/px4_platform_common/log.h b/platforms/common/include/px4_platform_common/log.h index db759370bf95..7b36fcd50fb5 100644 --- a/platforms/common/include/px4_platform_common/log.h +++ b/platforms/common/include/px4_platform_common/log.h @@ -127,7 +127,6 @@ __END_DECLS __BEGIN_DECLS __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1]; -__EXPORT extern const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) __attribute__((format(printf, 3, 4))); __EXPORT void px4_log_raw(int level, const char *fmt, ...) @@ -168,15 +167,6 @@ __END_DECLS #define __px4__log_file_and_line_arg , __FILE__, __LINE__ #define __px4__log_end_fmt "\n" -#define PX4_ANSI_COLOR_RED "\x1b[31m" -#define PX4_ANSI_COLOR_GREEN "\x1b[32m" -#define PX4_ANSI_COLOR_YELLOW "\x1b[33m" -#define PX4_ANSI_COLOR_BLUE "\x1b[34m" -#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m" -#define PX4_ANSI_COLOR_CYAN "\x1b[36m" -#define PX4_ANSI_COLOR_GRAY "\x1B[37m" -#define PX4_ANSI_COLOR_RESET "\x1b[0m" - #ifdef __PX4_POSIX #define PX4_LOG_COLORIZED_OUTPUT //if defined and output is a tty, colorize the output according to the log level #endif /* __PX4_POSIX */ diff --git a/platforms/common/include/px4_platform_common/module.h b/platforms/common/include/px4_platform_common/module.h index 1304c9241aff..63867df4e2c8 100644 --- a/platforms/common/include/px4_platform_common/module.h +++ b/platforms/common/include/px4_platform_common/module.h @@ -362,17 +362,16 @@ class ModuleBase * @brief Waits until _object is initialized, (from the new thread). This can be called from task_spawn(). * @return Returns 0 iff successful, -1 on timeout or otherwise. */ - static int wait_until_running() + static int wait_until_running(int timeout_ms = 1000) { int i = 0; do { - /* Wait up to 1s. */ - px4_usleep(2500); + px4_usleep(2000); - } while (!_object.load() && ++i < 400); + } while (!_object.load() && ++i < timeout_ms / 2); - if (i == 400) { + if (i >= timeout_ms / 2) { PX4_ERR("Timed out while waiting for thread to start"); return -1; } diff --git a/platforms/common/include/px4_platform_common/module_params.h b/platforms/common/include/px4_platform_common/module_params.h index d2e518f0cebc..5a3d085fe7f9 100644 --- a/platforms/common/include/px4_platform_common/module_params.h +++ b/platforms/common/include/px4_platform_common/module_params.h @@ -61,9 +61,14 @@ class ModuleParams : public ListNode if (parent) { parent->_children.add(this); } + + _parent = parent; } - virtual ~ModuleParams() = default; + virtual ~ModuleParams() + { + if (_parent) { _parent->_children.remove(this); } + } // Disallow copy construction and move assignment. ModuleParams(const ModuleParams &) = delete; @@ -93,4 +98,5 @@ class ModuleParams : public ListNode private: /** @list _children The module parameter list of inheriting classes. */ List _children; + ModuleParams *_parent{nullptr}; }; diff --git a/platforms/common/include/px4_platform_common/printload.h b/platforms/common/include/px4_platform_common/printload.h index e72b13633a24..9bf9e7c80e91 100644 --- a/platforms/common/include/px4_platform_common/printload.h +++ b/platforms/common/include/px4_platform_common/printload.h @@ -45,8 +45,8 @@ #include -#ifndef CONFIG_MAX_TASKS -#define CONFIG_MAX_TASKS 64 +#ifndef CONFIG_FS_PROCFS_MAX_TASKS +#define CONFIG_FS_PROCFS_MAX_TASKS 64 #endif struct print_load_s { @@ -57,7 +57,7 @@ struct print_load_s { uint64_t new_time{0}; uint64_t interval_start_time{0}; - uint64_t last_times[CONFIG_MAX_TASKS] {}; + uint64_t last_times[CONFIG_FS_PROCFS_MAX_TASKS] {}; float interval_time_us{0.f}; }; diff --git a/platforms/common/include/px4_platform_common/px4_config.h b/platforms/common/include/px4_platform_common/px4_config.h index aac0a9be5261..f66304bf78f6 100644 --- a/platforms/common/include/px4_platform_common/px4_config.h +++ b/platforms/common/include/px4_platform_common/px4_config.h @@ -52,3 +52,6 @@ #include #endif + +/* PX4 board kconfig symbols */ +#include diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 2f95b064c4ed..5c1f82061dca 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -79,7 +79,7 @@ class WorkItem : public IntrusiveSortedListNode, public IntrusiveQue * @param config The WorkQueue configuration (see WorkQueueManager.hpp). * @return true if initialization was successful */ - bool ChangeWorkQeue(const wq_config_t &config) { return Init(config); } + bool ChangeWorkQueue(const wq_config_t &config) { return Init(config); } const char *ItemName() const { return _item_name; } diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index d1ad917cdd75..f2a66bb8983c 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -96,6 +96,7 @@ class WorkQueue : public IntrusiveSortedListNode IntrusiveQueue _q; px4_sem_t _process_lock; + px4_sem_t _exit_lock; const wq_config_t &_config; BlockingList _work_items; px4::atomic_bool _should_exit{false}; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 441b5c7002ca..94614bf5d824 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -48,8 +48,7 @@ struct wq_config_t { namespace wq_configurations { -static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1920, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t ctrl_alloc{"wq:ctrl_alloc", 9500, 0}; // PX4 control allocation, same priority as rate_ctrl +static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 3150, 0}; // PX4 inner loop highest priority static constexpr wq_config_t SPI0{"wq:SPI0", 2336, -1}; static constexpr wq_config_t SPI1{"wq:SPI1", 2336, -2}; @@ -66,7 +65,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2164, -13}; +static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2240, -13}; static constexpr wq_config_t INS0{"wq:INS0", 6000, -14}; static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; @@ -77,16 +76,18 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; -static constexpr wq_config_t UART0{"wq:UART0", 1504, -21}; -static constexpr wq_config_t UART1{"wq:UART1", 1504, -22}; -static constexpr wq_config_t UART2{"wq:UART2", 1504, -23}; -static constexpr wq_config_t UART3{"wq:UART3", 1504, -24}; -static constexpr wq_config_t UART4{"wq:UART4", 1504, -25}; -static constexpr wq_config_t UART5{"wq:UART5", 1504, -26}; -static constexpr wq_config_t UART6{"wq:UART6", 1504, -27}; -static constexpr wq_config_t UART7{"wq:UART7", 1504, -28}; -static constexpr wq_config_t UART8{"wq:UART8", 1504, -29}; -static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1504, -30}; +static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21}; +static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22}; +static constexpr wq_config_t ttyS2{"wq:ttyS2", 1632, -23}; +static constexpr wq_config_t ttyS3{"wq:ttyS3", 1632, -24}; +static constexpr wq_config_t ttyS4{"wq:ttyS4", 1632, -25}; +static constexpr wq_config_t ttyS5{"wq:ttyS5", 1632, -26}; +static constexpr wq_config_t ttyS6{"wq:ttyS6", 1632, -27}; +static constexpr wq_config_t ttyS7{"wq:ttyS7", 1632, -28}; +static constexpr wq_config_t ttyS8{"wq:ttyS8", 1632, -29}; +static constexpr wq_config_t ttyS9{"wq:ttyS9", 1632, -30}; +static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1632, -31}; +static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1632, -32}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; diff --git a/platforms/common/include/px4_platform_common/shmem.h b/platforms/common/include/px4_platform_common/shmem.h deleted file mode 100644 index 7d7d18498818..000000000000 --- a/platforms/common/include/px4_platform_common/shmem.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Ramakrishna Kintada. 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 - -#define MAX_SHMEM_PARAMS 2000 //MAP_SIZE - (LOCK_SIZE - sizeof(struct shmem_info)) - -#define PARAM_BUFFER_SIZE (MAX_SHMEM_PARAMS / 8 + 1) - -struct shmem_info { - union param_value_u params_val[MAX_SHMEM_PARAMS]; - unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by krait*/ - unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; /*bit map of all params changed by adsp*/ - -#ifdef __PX4_NUTTX -}; -#else -} __attribute__((packed)); -#endif - -#if (defined(__PX4_POSIX_EXCELSIOR) || defined(__PX4_QURT_EXCELSIOR)) -#define MAP_ADDRESS 0x861FC000 -#else -#define MAP_ADDRESS 0xfbfc000 -#endif -#define MAP_SIZE 16384 -#define MAP_MASK (MAP_SIZE - 1) - -#define LOCK_OFFSET 0 -#define LOCK_SIZE 4 -#define LOCK_MEM 1 -#define UNLOCK_MEM 2 - -#define TYPE_MASK 0x1 - -extern bool handle_in_range(param_t); - -#ifdef __PX4_QURT -extern struct shmem_info *shmem_info_p; - -int get_shmem_lock(const char *caller_file_name, int caller_line_number); -void release_shmem_lock(const char *caller_file_name, int caller_line_number); -void init_shared_memory(void); - -void copy_params_to_shmem(const param_info_s *param_info_base); -#endif - -void update_to_shmem(param_t param, union param_value_u value); -int update_from_shmem(param_t param, union param_value_u *value); -void update_index_from_shmem(void); diff --git a/platforms/common/include/px4_platform_common/shutdown.h b/platforms/common/include/px4_platform_common/shutdown.h index 0a7029f91e8e..f4bd2e9cf6b3 100644 --- a/platforms/common/include/px4_platform_common/shutdown.h +++ b/platforms/common/include/px4_platform_common/shutdown.h @@ -97,9 +97,9 @@ __EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = * @param delay_us optional delay in microseconds * @return 0 on success, <0 on error */ -#if defined(CONFIG_BOARDCTL_POWEROFF) || defined(__PX4_POSIX) +#if defined(BOARD_HAS_POWER_CONTROL) || defined(__PX4_POSIX) __EXPORT int px4_shutdown_request(uint32_t delay_us = 0); -#endif // CONFIG_BOARDCTL_POWEROFF +#endif // BOARD_HAS_POWER_CONTROL /** diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index aa7189574407..b9e56bb900b5 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -36,6 +36,8 @@ #include #include +#if defined(CONFIG_SPI) + /* * Helper macros to handle device ID's. They are used to match drivers against SPI buses and chip-select signals. * They match with corresponding definitions in NuttX. @@ -71,7 +73,7 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version), -1=unused + int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 @@ -139,12 +141,14 @@ class SPIBusIterator * Constructor * Note: only for devices of type PX4_SPI_DEVICE_ID * @param filter - * @param devid_driver_index DRV_* or chip-select index starting from 1 + * @param devid_driver_index DRV_* + * @param chipselect pin of SPIInternal (-1=all) or chip-select index of SPIExternal starting from 1 (optional) * @param bus starts with 1 (-1=all, but only for internal). Numbering for internal is arch-specific, for external * it is the n-th external bus. */ - SPIBusIterator(FilterType filter, uint16_t devid_driver_index, int bus = -1) + SPIBusIterator(FilterType filter, uint16_t devid_driver_index, int16_t chipselect = -1, int bus = -1) : _filter(filter), _devid_driver_index(devid_driver_index), + _chipselect(chipselect), _bus(filter == FilterType::ExternalBus && bus == -1 ? 1 : bus) {} bool next(); @@ -158,11 +162,16 @@ class SPIBusIterator bool external() const { return px4_spi_bus_external(bus()); } + int busDeviceIndex() const { return _bus_device_index; } + private: const FilterType _filter; const uint16_t _devid_driver_index; + const int16_t _chipselect; const int _bus; - int _index{-1}; - int _external_bus_counter{0}; - int _bus_device_index{0}; + int _index{0}; + int _external_bus_counter{1}; + int _bus_device_index{-1}; }; + +#endif // CONFIG_SPI diff --git a/platforms/common/include/px4_platform_common/workqueue.h b/platforms/common/include/px4_platform_common/workqueue.h index b4fa7ad1a33d..8b7ee241639d 100644 --- a/platforms/common/include/px4_platform_common/workqueue.h +++ b/platforms/common/include/px4_platform_common/workqueue.h @@ -45,10 +45,6 @@ #include #include -#ifdef __PX4_QURT -#include -#endif - __BEGIN_DECLS #define HPWORK 0 diff --git a/platforms/common/include/px4_random.h b/platforms/common/include/px4_random.h new file mode 100644 index 000000000000..816bee16f387 --- /dev/null +++ b/platforms/common/include/px4_random.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +/* + * Random number generator provided by the operating system. This can be used + * by the crypto_arch if needed, or it can implement an own HW-specific rng + */ + +__BEGIN_DECLS +size_t px4_get_secure_random(uint8_t *out, + size_t outlen); +__END_DECLS diff --git a/platforms/common/module.cpp b/platforms/common/module.cpp index 3ca0b6a74897..8b474c794100 100644 --- a/platforms/common/module.cpp +++ b/platforms/common/module.cpp @@ -60,8 +60,12 @@ void PRINT_MODULE_DESCRIPTION(const char *description) void PRINT_MODULE_USAGE_NAME(const char *executable_name, const char *category) { +#ifndef CONSTRAINED_FLASH_NO_HELP PX4_INFO_RAW("Usage: %s [arguments...]\n", executable_name); PX4_INFO_RAW(" Commands:\n"); +#else + PX4_INFO_RAW("Usage: %s See:" CONSTRAINED_FLASH_NO_HELP "\n", executable_name); +#endif } void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory) @@ -71,26 +75,36 @@ void PRINT_MODULE_USAGE_SUBCATEGORY(const char *subcategory) void PRINT_MODULE_USAGE_NAME_SIMPLE(const char *executable_name, const char *category) { +#ifndef CONSTRAINED_FLASH_NO_HELP PX4_INFO_RAW("Usage: %s [arguments...]\n", executable_name); +#endif } void PRINT_MODULE_USAGE_COMMAND_DESCR(const char *name, const char *description) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (description) { PX4_INFO_RAW("\n %-13s %s\n", name, description); } else { PX4_INFO_RAW("\n %s\n", name); } + +#endif } void PRINT_MODULE_USAGE_PARAM_COMMENT(const char *comment) { +#ifndef CONSTRAINED_FLASH_NO_HELP PX4_INFO_RAW("\n %s\n", comment); +#endif } void PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(bool i2c_support, bool spi_support) { +#ifndef CONSTRAINED_FLASH_NO_HELP + // Note: this must be kept in sync with Tools/px4moduledoc/srcparser.py if (i2c_support) { PRINT_MODULE_USAGE_PARAM_FLAG('I', "Internal I2C bus(es)", true); @@ -106,27 +120,34 @@ void PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(bool i2c_support, bool spi_support true); if (spi_support) { - PRINT_MODULE_USAGE_PARAM_INT('c', 1, 1, 10, "chip-select index (for external SPI)", true); + PRINT_MODULE_USAGE_PARAM_INT('c', -1, 0, 31, "chip-select pin (for internal SPI) or index (for external SPI)", true); PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 3, "SPI mode", true); } PRINT_MODULE_USAGE_PARAM_INT('f', -1, 0, 100000, "bus frequency in kHz", true); PRINT_MODULE_USAGE_PARAM_FLAG('q', "quiet startup (no message if no device found)", true); +#endif } void PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(uint8_t default_address) { +#ifndef CONSTRAINED_FLASH_NO_HELP PRINT_MODULE_USAGE_PARAM_INT('a', default_address, 0, 0xff, "I2C address", true); +#endif } void PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG() { +#ifndef CONSTRAINED_FLASH_NO_HELP PRINT_MODULE_USAGE_PARAM_FLAG('k', "if initialization (probing) fails, keep retrying periodically", true); +#endif } void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val, int max_val, const char *description, bool is_optional) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (is_optional) { PX4_INFO_RAW(" [-%c ] %s\n", option_char, description); @@ -137,11 +158,15 @@ void PRINT_MODULE_USAGE_PARAM_INT(char option_char, int default_val, int min_val } else { PX4_INFO_RAW(" -%c %s\n", option_char, description); } + +#endif } void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float min_val, float max_val, const char *description, bool is_optional) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (is_optional) { PX4_INFO_RAW(" [-%c ] %s\n", option_char, description); @@ -152,21 +177,29 @@ void PRINT_MODULE_USAGE_PARAM_FLOAT(char option_char, float default_val, float m } else { PX4_INFO_RAW(" -%c %s\n", option_char, description); } + +#endif } void PRINT_MODULE_USAGE_PARAM_FLAG(char option_char, const char *description, bool is_optional) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (is_optional) { PX4_INFO_RAW(" [-%c] %s\n", option_char, description); } else { PX4_INFO_RAW(" -%c %s\n", option_char, description); } + +#endif } void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, const char *values, const char *description, bool is_optional) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (is_optional) { PX4_INFO_RAW(" [-%c ] %s\n", option_char, description); @@ -187,16 +220,22 @@ void PRINT_MODULE_USAGE_PARAM_STRING(char option_char, const char *default_val, PX4_INFO_RAW(" default: %s\n", default_val); } } + +#endif } void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is_optional) { +#ifndef CONSTRAINED_FLASH_NO_HELP + if (is_optional) { PX4_INFO_RAW(" [%-9s] %s\n", values, description); } else { PX4_INFO_RAW(" %-11s %s\n", values, description); } + +#endif } diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 803ed2f890ac..0883784fb2e5 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -45,6 +45,7 @@ #include #endif +#include #include #include #include @@ -52,68 +53,119 @@ static orb_advert_t orb_log_message_pub = nullptr; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; -__EXPORT const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] = -{ PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_RESET, PX4_ANSI_COLOR_YELLOW, PX4_ANSI_COLOR_RED, PX4_ANSI_COLOR_RED }; +#define PX4_ANSI_COLOR_RED "\x1b[31m" +#define PX4_ANSI_COLOR_GREEN "\x1b[32m" +#define PX4_ANSI_COLOR_YELLOW "\x1b[33m" +#define PX4_ANSI_COLOR_BLUE "\x1b[34m" +#define PX4_ANSI_COLOR_MAGENTA "\x1b[35m" +#define PX4_ANSI_COLOR_CYAN "\x1b[36m" +#define PX4_ANSI_COLOR_GRAY "\x1B[37m" +#define PX4_ANSI_COLOR_RESET "\x1b[0m" + +static constexpr const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] { + PX4_ANSI_COLOR_GREEN, // DEBUG + PX4_ANSI_COLOR_RESET, // INFO + PX4_ANSI_COLOR_YELLOW, // WARN + PX4_ANSI_COLOR_RED, // ERROR + PX4_ANSI_COLOR_RED // PANIC +}; void px4_log_initialize(void) { - assert(orb_log_message_pub == nullptr); - - /* we need to advertise with a valid message */ - struct log_message_s log_message; - log_message.timestamp = hrt_absolute_time(); - log_message.severity = 6; //info + // we need to advertise with a valid message + log_message_s log_message{}; + log_message.severity = 6; // info strcpy((char *)log_message.text, "initialized uORB logging"); - + log_message.timestamp = hrt_absolute_time(); orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH); - - if (!orb_log_message_pub) { - PX4_ERR("failed to advertise log_message"); - } } - -__EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) +__EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...) { + static constexpr ssize_t max_length = sizeof(log_message_s::text); + FILE *out = stdout; + +#if defined(PX4_LOG_COLORIZED_OUTPUT) bool use_color = true; +#endif // PX4_LOG_COLORIZED_OUTPUT -#ifdef __PX4_POSIX - out = get_stdout(&use_color); -#endif +#if defined(__PX4_POSIX) + bool isatty_ = false; + out = get_stdout(&isatty_); -#ifndef PX4_LOG_COLORIZED_OUTPUT - use_color = false; -#endif +#if defined(PX4_LOG_COLORIZED_OUTPUT) + use_color = isatty_; +#endif // PX4_LOG_COLORIZED_OUTPUT +#endif // PX4_POSIX if (level >= _PX4_LOG_LEVEL_INFO) { - if (use_color) { fputs(__px4_log_level_color[level], out); } + char buf[max_length + 1]; // same length as log_message_s::text, but add newline + ssize_t pos = 0; + +#if defined(PX4_LOG_COLORIZED_OUTPUT) + + if (use_color) { + pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]); + } + +#endif // PX4_LOG_COLORIZED_OUTPUT - fprintf(out, __px4__log_level_fmt __px4__log_level_arg(level)); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), __px4__log_level_fmt, __px4_log_level_str[level]); - if (use_color) { fputs(PX4_ANSI_COLOR_GRAY, out); } +#if defined(PX4_LOG_COLORIZED_OUTPUT) - fprintf(out, __px4__log_modulename_pfmt, moduleName); + if (use_color) { + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), PX4_ANSI_COLOR_GRAY); + } - if (use_color) { fputs(__px4_log_level_color[level], out); } +#endif // PX4_LOG_COLORIZED_OUTPUT + + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), __px4__log_modulename_pfmt, module_name); + +#if defined(PX4_LOG_COLORIZED_OUTPUT) + + if (use_color) { + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", __px4_log_level_color[level]); + } + +#endif // PX4_LOG_COLORIZED_OUTPUT va_list argptr; va_start(argptr, fmt); - vfprintf(out, fmt, argptr); + pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); va_end(argptr); - if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); } +#if defined(PX4_LOG_COLORIZED_OUTPUT) + + if (use_color) { + // alway reset color + const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1); + pos += sprintf(buf + sz, "%s\n", PX4_ANSI_COLOR_RESET); - fputc('\n', out); + } else +#endif // PX4_LOG_COLORIZED_OUTPUT + { + pos += sprintf(buf + math::min(pos, max_length - (ssize_t)1), "\n"); + } + + // ensure NULL termination (buffer is max_length + 1) + buf[max_length] = 0; + + fputs(buf, out); + +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + // Without flushing it's tricky to see stdout output when PX4 is started by + // a script like for the MAVSDK tests. + fflush(out); +#endif // CONFIG_ARCH_BOARD_PX4_SITL } /* publish an orb log message */ if (level >= _PX4_LOG_LEVEL_INFO && orb_log_message_pub) { //publish all messages - struct log_message_s log_message; - const unsigned max_length_pub = sizeof(log_message.text); - log_message.timestamp = hrt_absolute_time(); + log_message_s log_message; const uint8_t log_level_table[] = { 7, /* _PX4_LOG_LEVEL_DEBUG */ @@ -124,47 +176,65 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char * }; log_message.severity = log_level_table[level]; - unsigned pos = 0; + ssize_t pos = snprintf((char *)log_message.text, max_length, __px4__log_modulename_pfmt, module_name); va_list argptr; - - pos += snprintf((char *)log_message.text + pos, max_length_pub - pos, __px4__log_modulename_pfmt, moduleName); va_start(argptr, fmt); - pos += vsnprintf((char *)log_message.text + pos, max_length_pub - pos, fmt, argptr); + pos += vsnprintf((char *)log_message.text + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); va_end(argptr); - log_message.text[max_length_pub - 1] = 0; //ensure 0-termination - + log_message.text[max_length - 1] = 0; //ensure 0-termination + log_message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message); } - -#ifdef CONFIG_ARCH_BOARD_PX4_SITL - // Without flushing it's tricky to see stdout output when PX4 is started by - // a script like for the MAVSDK tests. - fflush(out); -#endif } __EXPORT void px4_log_raw(int level, const char *fmt, ...) { FILE *out = stdout; - bool use_color = true; #ifdef __PX4_POSIX + bool use_color = true; out = get_stdout(&use_color); #endif -#ifndef PX4_LOG_COLORIZED_OUTPUT - use_color = false; -#endif - if (level >= _PX4_LOG_LEVEL_INFO) { - if (use_color) { fputs(__px4_log_level_color[level], out); } + static constexpr ssize_t max_length = sizeof(log_message_s::text); + char buf[max_length + 1]; // same length as log_message_s::text, but add newline + ssize_t pos = 0; + +#if defined(PX4_LOG_COLORIZED_OUTPUT) + + if (use_color) { + pos += sprintf(buf + pos, "%s", __px4_log_level_color[level]); + } + +#endif // PX4_LOG_COLORIZED_OUTPUT va_list argptr; va_start(argptr, fmt); - vfprintf(out, fmt, argptr); + pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); va_end(argptr); - if (use_color) { fputs(PX4_ANSI_COLOR_RESET, out); } +#if defined(PX4_LOG_COLORIZED_OUTPUT) + + if (use_color) { + // alway reset color + const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET)); + pos += sprintf(buf + sz, "%s", PX4_ANSI_COLOR_RESET); + } + +#endif // PX4_LOG_COLORIZED_OUTPUT + + if (pos > max_length) { + // preserve newline if necessary + if (fmt[strlen(fmt) - 1] == '\n') { + buf[max_length - 1] = '\n'; + } + } + + // ensure NULL termination (buffer is max_length + 1) + buf[max_length] = 0; + + fputs(buf, out); } } diff --git a/platforms/common/px4_work_queue/CMakeLists.txt b/platforms/common/px4_work_queue/CMakeLists.txt index 429c69d6d141..01f8cc3db2b1 100644 --- a/platforms/common/px4_work_queue/CMakeLists.txt +++ b/platforms/common/px4_work_queue/CMakeLists.txt @@ -44,4 +44,3 @@ if(PX4_TESTING) endif() target_compile_options(px4_work_queue PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_link_libraries(px4_work_queue PRIVATE px4_platform) diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index 3c57ff45cda1..f5f9e130f079 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -38,7 +38,9 @@ namespace px4 ScheduledWorkItem::~ScheduledWorkItem() { - ScheduleClear(); + if (_call.arg != nullptr) { + ScheduleClear(); + } } void ScheduledWorkItem::schedule_trampoline(void *arg) @@ -72,7 +74,7 @@ void ScheduledWorkItem::ScheduleClear() void ScheduledWorkItem::print_run_status() { if (_call.period > 0) { - PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), + PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), (double)average_interval(), _call.period); } else { diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index 0c4be6a15bdd..ffc0dca2761f 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -134,7 +134,7 @@ float WorkItem::average_interval() const void WorkItem::print_run_status() { - PX4_INFO_RAW("%-26s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); + PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); // reset statistics _run_count = 0; diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index b6a172e7b864..fa32017912fd 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -49,7 +49,7 @@ WorkQueue::WorkQueue(const wq_config_t &config) : // set the threads name #ifdef __PX4_DARWIN pthread_setname_np(_config.name); -#elif !defined(__PX4_QURT) +#else pthread_setname_np(pthread_self(), _config.name); #endif @@ -59,11 +59,20 @@ WorkQueue::WorkQueue(const wq_config_t &config) : px4_sem_init(&_process_lock, 0, 0); px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE); + + px4_sem_init(&_exit_lock, 0, 1); + px4_sem_setprotocol(&_exit_lock, SEM_PRIO_NONE); } WorkQueue::~WorkQueue() { + work_lock(); + + // Synchronize with ::Detach + px4_sem_wait(&_exit_lock); + px4_sem_destroy(&_exit_lock); + px4_sem_destroy(&_process_lock); work_unlock(); @@ -83,11 +92,14 @@ bool WorkQueue::Attach(WorkItem *item) } work_unlock(); + return false; } void WorkQueue::Detach(WorkItem *item) { + bool exiting = false; + work_lock(); _work_items.remove(item); @@ -96,11 +108,21 @@ void WorkQueue::Detach(WorkItem *item) // shutdown, no active WorkItems PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name); + // Deletion of this work queue might happen right after request_stop or + // SignalWorkerThread. Use a separate lock to prevent premature deletion + px4_sem_wait(&_exit_lock); + exiting = true; request_stop(); SignalWorkerThread(); } work_unlock(); + + // In case someone is deleting this wq already, signal + // that it is now allowed + if (exiting) { + px4_sem_post(&_exit_lock); + } } void WorkQueue::Add(WorkItem *item) diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 7486e0bee4ff..95f9a47147a7 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -166,39 +166,45 @@ const wq_config_t & serial_port_to_wq(const char *serial) { if (serial == nullptr) { - return wq_configurations::hp_default; + return wq_configurations::ttyUnknown; } else if (strstr(serial, "ttyS0")) { - return wq_configurations::UART0; + return wq_configurations::ttyS0; } else if (strstr(serial, "ttyS1")) { - return wq_configurations::UART1; + return wq_configurations::ttyS1; } else if (strstr(serial, "ttyS2")) { - return wq_configurations::UART2; + return wq_configurations::ttyS2; } else if (strstr(serial, "ttyS3")) { - return wq_configurations::UART3; + return wq_configurations::ttyS3; } else if (strstr(serial, "ttyS4")) { - return wq_configurations::UART4; + return wq_configurations::ttyS4; } else if (strstr(serial, "ttyS5")) { - return wq_configurations::UART5; + return wq_configurations::ttyS5; } else if (strstr(serial, "ttyS6")) { - return wq_configurations::UART6; + return wq_configurations::ttyS6; } else if (strstr(serial, "ttyS7")) { - return wq_configurations::UART7; + return wq_configurations::ttyS7; } else if (strstr(serial, "ttyS8")) { - return wq_configurations::UART8; + return wq_configurations::ttyS8; + + } else if (strstr(serial, "ttyS9")) { + return wq_configurations::ttyS9; + + } else if (strstr(serial, "ttyACM0")) { + return wq_configurations::ttyACM0; } PX4_DEBUG("unknown serial port: %s", serial); - return wq_configurations::UART_UNKNOWN; + return wq_configurations::ttyUnknown; } const wq_config_t &ins_instance_to_wq(uint8_t instance) @@ -235,6 +241,17 @@ WorkQueueRunner(void *context) return nullptr; } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +// Wrapper for px4_task_spawn_cmd interface +inline static int +WorkQueueRunner(int argc, char *argv[]) +{ + wq_config_t *context = (wq_config_t *)strtoul(argv[argc - 1], nullptr, 16); + WorkQueueRunner(context); + return 0; +} +#endif + static int WorkQueueManagerRun(int, char **) { @@ -248,38 +265,44 @@ WorkQueueManagerRun(int, char **) if (wq != nullptr) { // create new work queue - pthread_attr_t attr; - int ret_attr_init = pthread_attr_init(&attr); - - if (ret_attr_init != 0) { - PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init); - } - - sched_param param; - int ret_getschedparam = pthread_attr_getschedparam(&attr, ¶m); - - if (ret_getschedparam != 0) { - PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); - } - // stack size #if defined(__PX4_QURT) const size_t stacksize = math::max(8 * 1024, PX4_STACK_ADJUSTED(wq->stacksize)); #elif defined(__PX4_NUTTX) - const size_t stacksize = math::max((uint16_t)PTHREAD_STACK_MIN, wq->stacksize); + const size_t stacksize = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize)); #elif defined(__PX4_POSIX) // On posix system , the desired stacksize round to the nearest multiplier of the system pagesize // It is a requirement of the pthread_attr_setstacksize* function const unsigned int page_size = sysconf(_SC_PAGESIZE); - const size_t stacksize_adj = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize)); + const size_t stacksize_adj = math::max((int)PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize)); const size_t stacksize = (stacksize_adj + page_size - (stacksize_adj % page_size)); #endif + + // priority + int sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority; + + // use pthreads for NuttX flat and posix builds. For NuttX protected build, use tasks or kernel threads +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) + pthread_attr_t attr; + int ret_attr_init = pthread_attr_init(&attr); + int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize); if (ret_setstacksize != 0) { PX4_ERR("setting stack size for %s failed (%i)", wq->name, ret_setstacksize); } + if (ret_attr_init != 0) { + PX4_ERR("attr init for %s failed (%i)", wq->name, ret_attr_init); + } + + sched_param param; + int ret_getschedparam = pthread_attr_getschedparam(&attr, ¶m); + + if (ret_getschedparam != 0) { + PX4_ERR("getting sched param for %s failed (%i)", wq->name, ret_getschedparam); + } + #ifndef __PX4_QURT // schedule policy FIFO @@ -292,7 +315,7 @@ WorkQueueManagerRun(int, char **) #endif // ! QuRT // priority - param.sched_priority = sched_get_priority_max(SCHED_FIFO) + wq->relative_priority; + param.sched_priority = sched_priority; int ret_setschedparam = pthread_attr_setschedparam(&attr, ¶m); if (ret_setschedparam != 0) { @@ -316,6 +339,30 @@ WorkQueueManagerRun(int, char **) if (ret_destroy != 0) { PX4_ERR("failed to destroy thread attributes for %s (%i)", wq->name, ret_create); } + +#else + // create thread + + // pack wq struct pointer into string, this is compatible with px4_task_spawn_cmd + char arg1[sizeof(void *) * 3]; + sprintf(arg1, "%lx", (long unsigned)wq); + const char *arg[2] = {arg1, nullptr}; + + int pid = px4_task_spawn_cmd(wq->name, + SCHED_FIFO, + sched_priority, + stacksize, + WorkQueueRunner, + (char *const *)arg); + + if (pid > 0) { + PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, sched_priority, stacksize); + + } else { + PX4_ERR("failed to create thread for %s (%i): %s", wq->name, pid, strerror(pid)); + } + +#endif } } @@ -332,7 +379,7 @@ WorkQueueManagerStart() int task_id = px4_task_spawn_cmd("wq:manager", SCHED_DEFAULT, SCHED_PRIORITY_MAX, - 1280, + PX4_STACK_ADJUSTED(1280), (px4_main_t)&WorkQueueManagerRun, nullptr); @@ -407,7 +454,7 @@ WorkQueueManagerStatus() if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) { const size_t num_wqs = _wq_manager_wqs_list->size(); - PX4_INFO_RAW("\nWork Queue: %-1zu threads RATE INTERVAL\n", num_wqs); + PX4_INFO_RAW("\nWork Queue: %-2zu threads RATE INTERVAL\n", num_wqs); LockGuard lg{_wq_manager_wqs_list->mutex()}; size_t i = 0; diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index b140e5f1a490..8e7c3d877d3b 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -56,6 +56,7 @@ #ifdef __PX4_NUTTX #include +#include #endif using namespace time_literals; @@ -99,7 +100,7 @@ int px4_shutdown_unlock() return ret; } -#if defined(CONFIG_SCHED_WORKQUEUE) +#if defined(CONFIG_SCHED_WORKQUEUE) || (!defined(CONFIG_BUILD_FLAT) && defined(CONFIG_LIB_USRWORK)) static struct work_s shutdown_work = {}; static uint16_t shutdown_counter = 0; ///< count how many times the shutdown worker was executed @@ -174,16 +175,20 @@ static void shutdown_worker(void *arg) if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); - board_reset((shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); + boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); #else PX4_PANIC("board reset not available"); #endif } else { -#if defined(CONFIG_BOARDCTL_POWEROFF) +#if defined(BOARD_HAS_POWER_CONTROL) PX4_INFO_RAW("Powering off NOW."); +#if defined(CONFIG_BOARDCTL_POWEROFF) + boardctl(BOARDIOC_POWEROFF, 0); +#else board_power_off(0); -#elif !defined(CONFIG_BOARDCTL_POWEROFF) && defined(__PX4_POSIX) +#endif +#elif defined(__PX4_POSIX) // simply exit on posix if real shutdown (poweroff) not available PX4_INFO_RAW("Exiting NOW."); system_exit(0); @@ -228,7 +233,7 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us) } #endif // CONFIG_BOARDCTL_RESET -#if defined(CONFIG_BOARDCTL_POWEROFF) || defined(__PX4_POSIX) +#if defined(BOARD_HAS_POWER_CONTROL) || defined(__PX4_POSIX) int px4_shutdown_request(uint32_t delay_us) { pthread_mutex_lock(&shutdown_mutex); @@ -250,6 +255,6 @@ int px4_shutdown_request(uint32_t delay_us) pthread_mutex_unlock(&shutdown_mutex); return 0; } -#endif // CONFIG_BOARDCTL_POWEROFF +#endif // BOARD_HAS_POWER_CONTROL #endif // CONFIG_SCHED_WORKQUEUE) diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index 1e6afe09a4c7..acce069ea37b 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -32,21 +32,31 @@ ****************************************************************************/ #include -#ifndef BOARD_DISABLE_I2C_SPI + +#if defined(CONFIG_SPI) #include +#ifndef GPIO_PIN_MASK +#define GPIO_PIN_MASK 0 +#endif + #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 void px4_set_spi_buses_from_hw_version() { - int hw_version = board_get_hw_version(); +#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) + int hw_version_revision = board_get_hw_version(); +#else + int hw_version_revision = (board_get_hw_version() << 8) | board_get_hw_revision(); +#endif + for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { - if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version == 0) { + if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) { px4_spi_buses = px4_spi_buses_all_hw[i].buses; } - if (px4_spi_buses_all_hw[i].board_hw_version == hw_version) { + if (px4_spi_buses_all_hw[i].board_hw_version_revision == hw_version_revision) { px4_spi_buses = px4_spi_buses_all_hw[i].buses; } } @@ -104,54 +114,61 @@ bool px4_spi_bus_external(const px4_spi_bus_t &bus) bool SPIBusIterator::next() { - // we have at most 1 match per bus, so we can directly jump to the next bus - while (++_index < SPI_BUS_MAX_BUS_ITEMS && px4_spi_buses[_index].bus != -1) { + while (_index < SPI_BUS_MAX_BUS_ITEMS && px4_spi_buses[_index].bus != -1) { const px4_spi_bus_t &bus_data = px4_spi_buses[_index]; - if (!board_has_bus(BOARD_SPI_BUS, bus_data.bus)) { - continue; - } - - // Note: we use bus_data.is_external here instead of px4_spi_bus_external(), - // otherwise the chip-select matching does not work if a bus is configured as - // external/internal, but at runtime the other way around. - // (On boards where a bus can be internal/external at runtime, it should be - // configured as external.) - switch (_filter) { - case FilterType::InternalBus: - if (!bus_data.is_external) { - if (_bus == bus_data.bus || _bus == -1) { - // find device id - for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { - if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(bus_data.devices[i].devid) && - _devid_driver_index == bus_data.devices[i].devtype_driver) { - _bus_device_index = i; - return true; + if (board_has_bus(BOARD_SPI_BUS, bus_data.bus)) { + + // Note: we use bus_data.is_external here instead of px4_spi_bus_external(), + // otherwise the chip-select matching does not work if a bus is configured as + // external/internal, but at runtime the other way around. + // (On boards where a bus can be internal/external at runtime, it should be + // configured as external.) + switch (_filter) { + case FilterType::InternalBus: + if (!bus_data.is_external) { + if (_bus == bus_data.bus || _bus == -1) { + // Note: if chipselect < 0, it's not defined and used in filter + // find device + for (int i = _bus_device_index + 1; i < SPI_BUS_MAX_DEVICES; ++i) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(bus_data.devices[i].devid) && + _devid_driver_index == bus_data.devices[i].devtype_driver && + (_chipselect < 0 || _chipselect == (int16_t)(bus_data.devices[i].cs_gpio & GPIO_PIN_MASK))) { + _bus_device_index = i; + return true; + } } } } - } - break; + break; - case FilterType::ExternalBus: - if (bus_data.is_external) { - ++_external_bus_counter; - uint16_t cs_index = _devid_driver_index - 1; + case FilterType::ExternalBus: + if (bus_data.is_external) { + // Note: chip-select index is starting from 1 in CLI, -1 means not defined + uint16_t cs_index = _chipselect < 1 ? 0 : _chipselect - 1; - if (_bus == _external_bus_counter && cs_index < SPI_BUS_MAX_DEVICES && - bus_data.devices[cs_index].cs_gpio != 0) { - // we know that bus_data.devices[cs_index].devtype_driver == cs_index - _bus_device_index = cs_index; - return true; + if (_bus == _external_bus_counter && cs_index < SPI_BUS_MAX_DEVICES && + bus_data.devices[cs_index].cs_gpio != 0 && cs_index != _bus_device_index) { + // we know that bus_data.devices[cs_index].devtype_driver == cs_index + _bus_device_index = cs_index; + return true; + } } + + break; } - break; + if (bus_data.is_external) { + ++_external_bus_counter; + } } + + ++_index; + _bus_device_index = -1; } return false; } -#endif /* BOARD_DISABLE_I2C_SPI */ +#endif // CONFIG_SPI diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 5838d8c40eff..0f0060836b93 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,7 +34,9 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) -px4_add_library(uORB +set(SRCS) + +set(SRCS_COMMON ORBSet.hpp Publication.hpp PublicationMulti.hpp @@ -47,18 +49,49 @@ px4_add_library(uORB uORB.h uORBCommon.hpp uORBCommunicator.hpp - uORBDeviceMaster.cpp - uORBDeviceMaster.hpp - uORBDeviceNode.cpp - uORBDeviceNode.hpp - uORBManager.cpp uORBManager.hpp uORBUtils.cpp uORBUtils.hpp -) + uORBDeviceMaster.hpp + uORBDeviceNode.hpp + ) + +set(SRCS_KERNEL + uORBDeviceMaster.cpp + uORBDeviceNode.cpp + uORBManager.cpp + ) + +set(SRCS_USER + uORBManagerUsr.cpp + ) + +if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") + # Kernel side library in nuttx kernel/protected build + px4_add_library(uORB_kernel + ${SRCS_COMMON} + ${SRCS_KERNEL} + ) + target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs) + target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__) + + # User side library in nuttx kernel/protected build + px4_add_library(uORB + ${SRCS_COMMON} + ${SRCS_USER} + ) +else() + + # Library for all other targets (flat build, posix...) + px4_add_library(uORB + ${SRCS_COMMON} + ${SRCS_KERNEL} + ) + target_link_libraries(uORB PRIVATE cdev) +endif() +target_link_libraries(uORB PRIVATE uorb_msgs) target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) -target_link_libraries(uORB PRIVATE cdev uorb_msgs) if(PX4_TESTING) add_subdirectory(uORB_tests) diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index bb95f2133468..7547e3b7faf3 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -42,7 +42,7 @@ #include #include -#include "uORBDeviceNode.hpp" +#include "uORBManager.hpp" #include namespace uORB @@ -72,7 +72,7 @@ class PublicationBase bool advertised() const { return _handle != nullptr; } - bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); } + bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); } orb_id_t get_topic() const { return get_orb_meta(_orb_id); } @@ -84,7 +84,7 @@ class PublicationBase { if (_handle != nullptr) { // don't automatically unadvertise queued publications (eg vehicle_command) - if (static_cast(_handle)->get_queue_size() == 1) { + if (Manager::orb_get_queue_size(_handle) == 1) { unadvertise(); } } @@ -129,7 +129,7 @@ class Publication : public PublicationBase advertise(); } - return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK); + return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK); } }; diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index 7a91bc907e60..bc275940b604 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -96,7 +96,7 @@ class PublicationMulti : public PublicationBase { // advertise if not already advertised if (advertise()) { - return static_cast(_handle)->get_instance(); + return Manager::orb_get_instance(_handle); } return -1; diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 09acd60702fa..9c17cab378d2 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -49,26 +49,14 @@ bool Subscription::subscribe() return true; } - if (_orb_id != ORB_ID::INVALID) { + if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) { + unsigned initial_generation; + void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation); - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - - if (device_master != nullptr) { - - if (!device_master->deviceNodeExists(_orb_id, _instance)) { - return false; - } - - uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance); - - if (node != nullptr) { - _node = node; - _node->add_internal_subscriber(); - - _last_generation = _node->get_initial_generation(); - - return true; - } + if (node) { + _node = node; + _last_generation = initial_generation; + return true; } } @@ -78,7 +66,7 @@ bool Subscription::subscribe() void Subscription::unsubscribe() { if (_node != nullptr) { - _node->remove_internal_subscriber(); + uORB::Manager::orb_remove_internal_subscriber(_node); } _node = nullptr; @@ -88,13 +76,7 @@ void Subscription::unsubscribe() bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - - if (device_master != nullptr) { - if (!device_master->deviceNodeExists(_orb_id, _instance)) { - return false; - } - + if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 8e082d2a601b..496b8cf261e3 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -44,7 +44,6 @@ #include #include -#include "uORBDeviceNode.hpp" #include "uORBManager.hpp" #include "uORBUtils.hpp" @@ -120,14 +119,14 @@ class Subscription bool advertised() { if (valid()) { - return _node->is_advertised(); + return Manager::is_advertised(_node); } // try to initialize if (subscribe()) { // check again if valid if (valid()) { - return _node->is_advertised(); + return Manager::is_advertised(_node); } } @@ -137,19 +136,40 @@ class Subscription /** * Check if there is a new update. */ - bool updated() { return advertised() && _node->updates_available(_last_generation); } + bool updated() + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::updates_available(_node, _last_generation) : false; + } /** * Update the struct * @param dst The uORB message struct we are updating. */ - bool update(void *dst) { return updated() && _node->copy(dst, _last_generation); } + bool update(void *dst) + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false; + } /** * Copy the struct * @param dst The uORB message struct we are updating. */ - bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); } + bool copy(void *dst) + { + if (!valid()) { + subscribe(); + } + + return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false; + } /** * Change subscription instance @@ -166,9 +186,9 @@ class Subscription friend class SubscriptionCallback; friend class SubscriptionCallbackWorkItem; - DeviceNode *get_node() { return _node; } + void *get_node() { return _node; } - DeviceNode *_node{nullptr}; + void *_node{nullptr}; unsigned _last_generation{0}; /**< last generation the subscriber has seen */ diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 7f66d2396be4..942a9fbe17d3 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -69,7 +69,7 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNoderegister_callback(this)) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { // registered _registered = true; @@ -79,7 +79,7 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNoderegister_callback(this)) { + if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) { _registered = true; } } @@ -94,7 +94,7 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNodeunregister_callback(this); + Manager::unregister_callback(_subscription.get_node(), this); } _registered = false; @@ -164,7 +164,7 @@ class SubscriptionCallbackWorkItem : public SubscriptionCallback { // schedule immediately if updated (queue depth or subscription interval) if ((_required_updates == 0) - || (_subscription.get_node()->updates_available(_subscription.get_last_generation()) >= _required_updates)) { + || (Manager::updates_available(_subscription.get_node(), _subscription.get_last_generation()) >= _required_updates)) { if (updated()) { _work_item->ScheduleNow(); } diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 36e629f26d66..31d1b0a7af12 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -152,6 +152,11 @@ class SubscriptionInterval */ void set_interval_ms(uint32_t interval) { _interval_us = interval * 1000; } + /** + * Set the last data update + * @param t should be in range [now, now - _interval_us] + */ + void set_last_update(hrt_abstime t) { _last_update = t; } protected: Subscription _subscription; diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 7e3b96904d42..61b59d84ee24 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -41,6 +41,15 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" + +#include +#include +#include + +#ifdef __PX4_NUTTX +#include +#endif + static uORB::DeviceMaster *g_dev = nullptr; int uorb_start(void) @@ -56,6 +65,7 @@ int uorb_start(void) return -ENOMEM; } +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) /* create the driver */ g_dev = uORB::Manager::get_instance()->get_device_master(); @@ -63,11 +73,15 @@ int uorb_start(void) return -errno; } +#endif + return OK; } int uorb_status(void) { +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) + if (g_dev != nullptr) { g_dev->printStatistics(); @@ -75,11 +89,16 @@ int uorb_status(void) PX4_INFO("uorb is not running"); } +#else + boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS); +#endif return OK; } int uorb_top(char **topic_filter, int num_filters) { +#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) + if (g_dev != nullptr) { g_dev->showTop(topic_filter, num_filters); @@ -87,10 +106,12 @@ int uorb_top(char **topic_filter, int num_filters) PX4_INFO("uorb is not running"); } +#else + boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP); +#endif return OK; } - orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { return uORB::Manager::get_instance()->orb_advertise(meta, data); @@ -117,42 +138,42 @@ int orb_unadvertise(orb_advert_t handle) return uORB::Manager::get_instance()->orb_unadvertise(handle); } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { return uORB::Manager::get_instance()->orb_publish(meta, handle, data); } -int orb_subscribe(const struct orb_metadata *meta) +int orb_subscribe(const struct orb_metadata *meta) { return uORB::Manager::get_instance()->orb_subscribe(meta); } -int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } -int orb_unsubscribe(int handle) +int orb_unsubscribe(int handle) { return uORB::Manager::get_instance()->orb_unsubscribe(handle); } -int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) { return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); } -int orb_check(int handle, bool *updated) +int orb_check(int handle, bool *updated) { return uORB::Manager::get_instance()->orb_check(handle, updated); } -int orb_exists(const struct orb_metadata *meta, int instance) +int orb_exists(const struct orb_metadata *meta, int instance) { return uORB::Manager::get_instance()->orb_exists(meta, instance); } -int orb_group_count(const struct orb_metadata *meta) +int orb_group_count(const struct orb_metadata *meta) { unsigned instance = 0; @@ -172,3 +193,302 @@ int orb_get_interval(int handle, unsigned *interval) { return uORB::Manager::get_instance()->orb_get_interval(handle, interval); } + +const char *orb_get_c_type(unsigned char short_type) +{ + // this matches with the uorb o_fields generator + switch (short_type) { + case 0x82: return "int8_t"; + + case 0x83: return "int16_t"; + + case 0x84: return "int32_t"; + + case 0x85: return "int64_t"; + + case 0x86: return "uint8_t"; + + case 0x87: return "uint16_t"; + + case 0x88: return "uint32_t"; + + case 0x89: return "uint64_t"; + + case 0x8a: return "float"; + + case 0x8b: return "double"; + + case 0x8c: return "bool"; + + case 0x8d: return "char"; + } + + return nullptr; +} + + +void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name) +{ + if (print_topic_name) { + PX4_INFO_RAW(" %s\n", meta->o_name); + } + + const hrt_abstime now = hrt_absolute_time(); + hrt_abstime topic_timestamp = 0; + + const uint8_t *data_ptr = (const uint8_t *)data; + int data_offset = 0; + + for (int format_idx = 0; meta->o_fields[format_idx] != 0;) { + const char *end_field = strchr(meta->o_fields + format_idx, ';'); + + if (!end_field) { + PX4_ERR("Format error in %s", meta->o_fields); + return; + } + + const char *c_type = orb_get_c_type(meta->o_fields[format_idx]); + const int end_field_idx = end_field - meta->o_fields; + + int array_idx = -1; + int field_name_idx = -1; + + for (int field_idx = format_idx; field_idx != end_field_idx; ++field_idx) { + if (meta->o_fields[field_idx] == '[') { + array_idx = field_idx + 1; + + } else if (meta->o_fields[field_idx] == ' ') { + field_name_idx = field_idx + 1; + break; + } + } + + int array_size = 1; + + if (array_idx >= 0) { + array_size = strtol(meta->o_fields + array_idx, nullptr, 10); + } + + char field_name[80]; + size_t field_name_len = end_field_idx - field_name_idx; + + if (field_name_len >= sizeof(field_name)) { + PX4_ERR("field name too long %s (max: %u)", meta->o_fields, (unsigned)sizeof(field_name)); + return; + } + + memcpy(field_name, meta->o_fields + field_name_idx, field_name_len); + field_name[field_name_len] = '\0'; + + if (c_type) { // built-in type + bool dont_print = false; + + // handle special cases + if (strncmp(field_name, "_padding", 8) == 0) { + dont_print = true; + + } else if (strcmp(c_type, "char") == 0 && array_size > 1) { // string + PX4_INFO_RAW(" %s: \"%.*s\"\n", field_name, array_size, (char *)(data_ptr + data_offset)); + dont_print = true; + } + + if (!dont_print) { + PX4_INFO_RAW(" %s: ", field_name); + } + + if (!dont_print && array_size > 1) { + PX4_INFO_RAW("["); + } + + const int previous_data_offset = data_offset; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-align" // the caller ensures data is aligned + + for (int i = 0; i < array_size; ++i) { + if (strcmp(c_type, "int8_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi8, *(int8_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int8_t); + + } else if (strcmp(c_type, "int16_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi16, *(int16_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int16_t); + + } else if (strcmp(c_type, "int32_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi32, *(int32_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int32_t); + + } else if (strcmp(c_type, "int64_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIi64, *(int64_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(int64_t); + + } else if (strcmp(c_type, "uint8_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu8, *(uint8_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint8_t); + + } else if (strcmp(c_type, "uint16_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu16, *(uint16_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint16_t); + + } else if (strcmp(c_type, "uint32_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu32, *(uint32_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint32_t); + + } else if (strcmp(c_type, "uint64_t") == 0) { + if (!dont_print) { PX4_INFO_RAW("%" PRIu64, *(uint64_t *)(data_ptr + data_offset)); } + + data_offset += sizeof(uint64_t); + + } else if (strcmp(c_type, "float") == 0) { + if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); } + + data_offset += sizeof(float); + + } else if (strcmp(c_type, "double") == 0) { + if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); } + + data_offset += sizeof(double); + + } else if (strcmp(c_type, "bool") == 0) { + if (!dont_print) { PX4_INFO_RAW("%s", *(bool *)(data_ptr + data_offset) ? "True" : "False"); } + + data_offset += sizeof(bool); + + } else if (strcmp(c_type, "char") == 0) { + if (!dont_print) { PX4_INFO_RAW("%i", (int) * (char *)(data_ptr + data_offset)); } + + data_offset += sizeof(char); + + } else { + PX4_ERR("unknown type: %s", c_type); + return; + } + + if (!dont_print && i < array_size - 1) { + PX4_INFO_RAW(", "); + } + } + + if (!dont_print && array_size > 1) { + PX4_INFO_RAW("]"); + } + + // handle special cases + if (array_size == 1) { + if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp") == 0) { + topic_timestamp = *(uint64_t *)(data_ptr + previous_data_offset); + + if (topic_timestamp != 0) { + PX4_INFO_RAW(" (%.6f seconds ago)", (double)((now - topic_timestamp) / 1e6f)); + } + + } else if (strcmp(c_type, "uint64_t") == 0 && strcmp(field_name, "timestamp_sample") == 0) { + hrt_abstime timestamp = *(uint64_t *)(data_ptr + previous_data_offset); + + if (topic_timestamp != 0 && timestamp != 0) { + PX4_INFO_RAW(" (%i us before timestamp)", (int)(topic_timestamp - timestamp)); + } + + } else if (strstr(field_name, "flags") != nullptr) { + // bitfield + unsigned field_size = 0; + unsigned long value = 0; + + if (strcmp(c_type, "uint8_t") == 0) { + field_size = sizeof(uint8_t); + value = *(uint8_t *)(data_ptr + previous_data_offset); + + } else if (strcmp(c_type, "uint16_t") == 0) { + field_size = sizeof(uint16_t); + value = *(uint16_t *)(data_ptr + previous_data_offset); + + } else if (strcmp(c_type, "uint32_t") == 0) { + field_size = sizeof(uint32_t); + value = *(uint32_t *)(data_ptr + previous_data_offset); + } + + if (field_size > 0) { + PX4_INFO_RAW(" (0b"); + + for (int i = (field_size * 8) - 1; i >= 0; i--) { + PX4_INFO_RAW("%lu%s", (value >> i) & 1, ((unsigned)i < (field_size * 8) - 1 && i % 4 == 0 && i > 0) ? "'" : ""); + } + + PX4_INFO_RAW(")"); + } + + } else if (strcmp(c_type, "uint32_t") == 0 && strstr(field_name, "device_id") != nullptr) { + // Device ID + uint32_t device_id = *(uint32_t *)(data_ptr + previous_data_offset); + char device_id_buffer[80]; + device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), device_id); + PX4_INFO_RAW(" (%s)", device_id_buffer); + } + + } else if (array_size == 4 && strcmp(c_type, "float") == 0 && (strcmp(field_name, "q") == 0 + || strncmp(field_name, "q_", 2) == 0)) { + // attitude + float *attitude = (float *)(data_ptr + previous_data_offset); + matrix::Eulerf euler{matrix::Quatf{attitude}}; + PX4_INFO_RAW(" (Roll: %.1f deg, Pitch: %.1f deg, Yaw: %.1f deg)", + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + } + +#pragma GCC diagnostic pop + + PX4_INFO_RAW("\n"); + + } else { + + // extract the topic name + char topic_name[80]; + const size_t topic_name_len = array_size > 1 ? array_idx - format_idx - 1 : field_name_idx - format_idx - 1; + + if (topic_name_len >= sizeof(topic_name)) { + PX4_ERR("topic name too long in %s (max: %u)", meta->o_name, (unsigned)sizeof(topic_name)); + return; + } + + memcpy(topic_name, meta->o_fields + format_idx, topic_name_len); + topic_name[topic_name_len] = '\0'; + + // find the metadata + const orb_metadata *const *topics = orb_get_topics(); + const orb_metadata *found_topic = nullptr; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topics[i]->o_name, topic_name) == 0) { + found_topic = topics[i]; + break; + } + } + + if (!found_topic) { + PX4_ERR("Topic %s did not match any known topics", topic_name); + return; + } + + // print recursively + for (int i = 0; i < array_size; ++i) { + PX4_INFO_RAW(" %s", field_name); + + if (array_size > 1) { + PX4_INFO_RAW("[%i]", i); + } + + PX4_INFO_RAW(" (%s):\n", topic_name); + orb_print_message_internal(found_topic, data_ptr + data_offset, false); + data_offset += found_topic->o_size; + } + } + + format_idx = end_field_idx + 1; + } +} diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 95061c37f44a..c96f3c08d45d 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -31,8 +31,7 @@ * ****************************************************************************/ -#ifndef _UORB_UORB_H -#define _UORB_UORB_H +#pragma once /** * @file uORB.h @@ -235,6 +234,20 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT; */ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; +/** + * Returns the C type string from a short type in o_fields metadata, or nullptr + * if not a short type + */ +const char *orb_get_c_type(unsigned char short_type); + +/** + * Print a topic to console. Do not call this directly, use print_message() instead. + * @param meta orb topic metadata + * @param data expected to be aligned to the largest member + */ +void orb_print_message_internal(const struct orb_metadata *meta, const void *data, bool print_topic_name); + + __END_DECLS /* Diverse uORB header defines */ //XXX: move to better location @@ -245,4 +258,3 @@ typedef uint8_t hil_state_t; typedef uint8_t navigation_state_t; typedef uint8_t switch_pos_t; -#endif /* _UORB_UORB_H */ diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp index 3998db275e6e..e753a270da93 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/platforms/common/uORB/uORBDeviceMaster.cpp @@ -99,19 +99,11 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver *instance = group_tries; } - /* driver wants a permanent copy of the path, so make one here */ - const char *devpath = strdup(nodepath); - - if (devpath == nullptr) { - return -ENOMEM; - } - /* construct the new node, passing the ownership of path to it */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath); + uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, nodepath); - /* if we didn't get a device, that's bad, free the path too */ + /* if we didn't get a device, that's bad */ if (node == nullptr) { - free((void *)devpath); return -ENOMEM; } @@ -369,7 +361,9 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) if (!quit) { - //update the stats + // update the stats + int total_size = 0; + int total_msgs = 0; hrt_abstime current_time = hrt_absolute_time(); float dt = (current_time - start_time) / 1.e6f; cur_node = first_node; @@ -378,6 +372,10 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) unsigned int num_msgs = cur_node->node->updates_available(cur_node->last_pub_msg_count); cur_node->pub_msg_delta = roundf(num_msgs / dt); cur_node->last_pub_msg_count += num_msgs; + + total_size += cur_node->pub_msg_delta * cur_node->node->get_meta()->o_size; + total_msgs += cur_node->pub_msg_delta; + cur_node = cur_node->next; } @@ -388,7 +386,8 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) PX4_INFO_RAW("\033[H"); // move cursor to top left corner } - PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics); + PX4_INFO_RAW(CLEAR_LINE "update: 1s, topics: %i, total publications: %i, %.1f kB/s\n", + num_topics, total_msgs, (double)(total_size / 1000.f)); PX4_INFO_RAW(CLEAR_LINE "%-*s INST #SUB RATE #Q SIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME"); cur_node = first_node; @@ -404,6 +403,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) cur_node = cur_node->next; } + if (!only_once) { PX4_INFO_RAW("\033[0J"); // clear the rest of the screen } @@ -451,34 +451,6 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) return nullptr; } -bool uORB::DeviceMaster::deviceNodeExists(ORB_ID id, const uint8_t instance) -{ - if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) { - return false; - } - - return _node_exists[instance][(uint8_t)id]; -} - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) -{ - if (meta == nullptr) { - return nullptr; - } - - if (!deviceNodeExists(static_cast(meta->o_id), instance)) { - return nullptr; - } - - lock(); - uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); - unlock(); - - //We can safely return the node that can be used by any thread, because - //a DeviceNode never gets deleted. - return node; -} - uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance) { for (uORB::DeviceNode *node : _node_list) { diff --git a/platforms/common/uORB/uORBDeviceMaster.hpp b/platforms/common/uORB/uORBDeviceMaster.hpp index cf4f8de4cfd9..94c97e61dfc9 100644 --- a/platforms/common/uORB/uORBDeviceMaster.hpp +++ b/platforms/common/uORB/uORBDeviceMaster.hpp @@ -72,9 +72,34 @@ class uORB::DeviceMaster * @return node if exists, nullptr otherwise */ uORB::DeviceNode *getDeviceNode(const char *node_name); - uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance); - - bool deviceNodeExists(ORB_ID id, const uint8_t instance); + uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) + { + if (meta == nullptr) { + return nullptr; + } + + if (!deviceNodeExists(static_cast(meta->o_id), instance)) { + return nullptr; + } + + lock(); + uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); + unlock(); + + //We can safely return the node that can be used by any thread, because + //a DeviceNode never gets deleted. + return node; + + } + + bool deviceNodeExists(ORB_ID id, const uint8_t instance) + { + if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) { + return false; + } + + return _node_exists[instance][(uint8_t)id]; + } /** * Print statistics for each existing topic. diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 387feae1c79f..25dc68a04a4b 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -42,18 +42,11 @@ #include "uORBCommunicator.hpp" #endif /* ORB_COMMUNICATOR */ -static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } - -// Determine the data range -static inline bool is_in_range(unsigned left, unsigned value, unsigned right) -{ - if (right > left) { - return (left <= value) && (value <= right); +#if defined(__PX4_NUTTX) +#include +#endif - } else { // Maybe the data overflowed and a wraparound occurred - return (left <= value) || (value <= right); - } -} +static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } // round up to nearest power of two // Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128 @@ -82,7 +75,7 @@ static inline uint8_t round_pow_of_two_8(uint8_t n) uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size) : - CDev(path), + CDev(strdup(path)), // success is checked in CDev::init _meta(meta), _instance(instance), _queue_size(round_pow_of_two_8(queue_size)) @@ -91,9 +84,17 @@ uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t inst uORB::DeviceNode::~DeviceNode() { - delete[] _data; + free(_data); + + const char *devname = get_devname(); - CDev::unregister_driver_and_memory(); + if (devname) { +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + kmm_free((void *)devname); +#else + free((void *)devname); +#endif + } } int @@ -151,46 +152,6 @@ uORB::DeviceNode::close(cdev::file_t *filp) return CDev::close(filp); } -bool -uORB::DeviceNode::copy(void *dst, unsigned &generation) -{ - if ((dst != nullptr) && (_data != nullptr)) { - if (_queue_size == 1) { - ATOMIC_ENTER; - memcpy(dst, _data, _meta->o_size); - generation = _generation.load(); - ATOMIC_LEAVE; - return true; - - } else { - ATOMIC_ENTER; - const unsigned current_generation = _generation.load(); - - if (current_generation == generation) { - /* The subscriber already read the latest message, but nothing new was published yet. - * Return the previous message - */ - --generation; - } - - // Compatible with normal and overflow conditions - if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) { - // Reader is too far behind: some messages are lost - generation = current_generation - _queue_size; - } - - memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); - ATOMIC_LEAVE; - - ++generation; - - return true; - } - } - - return false; -} - ssize_t uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) { @@ -225,7 +186,9 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) /* re-check size */ if (nullptr == _data) { - _data = new uint8_t[_meta->o_size * _queue_size]; + const size_t data_size = _meta->o_size * _queue_size; + _data = (uint8_t *) px4_cache_aligned_alloc(data_size); + memset(_data, 0, data_size); } unlock(); @@ -323,7 +286,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v } /* check if the orb meta data matches the publication */ - if (devnode->_meta != meta) { + if (devnode->_meta->o_id != meta->o_id) { errno = EINVAL; return PX4_ERROR; } diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index e6bab775645b..bff59b38119c 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -225,7 +225,45 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_size); + generation = _generation.load(); + ATOMIC_LEAVE; + return true; + + } else { + ATOMIC_ENTER; + const unsigned current_generation = _generation.load(); + + if (current_generation == generation) { + /* The subscriber already read the latest message, but nothing new was published yet. + * Return the previous message + */ + --generation; + } + + // Compatible with normal and overflow conditions + if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) { + // Reader is too far behind: some messages are lost + generation = current_generation - _queue_size; + } + + memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); + ATOMIC_LEAVE; + + ++generation; + + return true; + } + } + + return false; + + } // add item to list of work items to schedule on node update bool register_callback(SubscriptionCallback *callback_sub); @@ -253,4 +291,16 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode left) { + return (left <= value) && (value <= right); + + } else { // Maybe the data overflowed and a wraparound occurred + return (left <= value) || (value <= right); + } + } }; diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 0493c68fd85a..0634c4e253f1 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -40,6 +40,10 @@ #include #include +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +#include +#endif + #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -52,6 +56,9 @@ bool uORB::Manager::initialize() _Instance = new uORB::Manager(); } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + px4_register_boardct_ioctl(_ORBIOCDEVBASE, orb_ioctl); +#endif return _Instance != nullptr; } @@ -103,6 +110,124 @@ uORB::DeviceMaster *uORB::Manager::get_device_master() return _device_master; } +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) +int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = PX4_OK; + + switch (cmd) { + case ORBIOCDEVEXISTS: { + orbiocdevexists_t *data = (orbiocdevexists_t *)arg; + + if (data->check_advertised) { + data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance); + + } else { + data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; + } + } + break; + + case ORBIOCDEVADVERTISE: { + orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); + + } else { + data->ret = PX4_ERROR; + } + } + break; + + case ORBIOCDEVUNADVERTISE: { + orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg; + data->ret = uORB::Manager::orb_unadvertise(data->handle); + } + break; + + case ORBIOCDEVPUBLISH: { + orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg; + data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data); + } + break; + + case ORBIOCDEVADDSUBSCRIBER: { + orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg; + data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation); + } + break; + + case ORBIOCDEVREMSUBSCRIBER: { + uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast(arg)); + } + break; + + case ORBIOCDEVQUEUESIZE: { + orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg; + data->size = uORB::Manager::orb_get_queue_size(data->handle); + } + break; + + case ORBIOCDEVDATACOPY: { + orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; + data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated); + } + break; + + case ORBIOCDEVREGCALLBACK: { + orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg; + data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVUNREGCALLBACK: { + orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg; + uORB::Manager::unregister_callback(data->handle, data->callback_sub); + } + break; + + case ORBIOCDEVGETINSTANCE: { + orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg; + data->instance = uORB::Manager::orb_get_instance(data->handle); + } + break; + + case ORBIOCDEVMASTERCMD: { + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + if (arg == ORB_DEVMASTER_TOP) { + dev->showTop(nullptr, 0); + + } else { + dev->printStatistics(); + } + } + } + break; + + case ORBIOCDEVUPDATESAVAIL: { + orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg; + data->ret = updates_available(data->handle, data->last_generation); + } + break; + + case ORBIOCDEVISADVERTISED: { + orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg; + data->ret = is_advertised(data->handle); + } + break; + + default: + ret = -ENOTTY; + } + + return ret; +} +#endif + int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { int ret = PX4_ERROR; @@ -112,8 +237,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return ret; } - if (get_device_master()) { - uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance); + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + + if (dev) { + uORB::DeviceNode *node = dev->getDeviceNode(meta, instance); if (node != nullptr) { if (node->is_advertised()) { @@ -319,6 +446,87 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval) return ret; } + +bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +{ + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + + return device_master != nullptr && + device_master->deviceNodeExists(orb_id, instance); +} + +void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) +{ + uORB::DeviceNode *node = nullptr; + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + + if (device_master != nullptr) { + node = device_master->getDeviceNode(get_orb_meta(orb_id), instance); + + if (node) { + node->add_internal_subscriber(); + *initial_generation = node->get_initial_generation(); + } + } + + return node; +} + +void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) +{ + static_cast(node_handle)->remove_internal_subscriber(); +} + +uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast(node_handle)->get_queue_size(); } + +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) +{ + if (!is_advertised(node_handle)) { + return false; + } + + if (only_if_updated && !static_cast(node_handle)->updates_available(generation)) { + return false; + } + + return static_cast(node_handle)->copy(dst, generation); +} + +// add item to list of work items to schedule on node update +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + return static_cast(node_handle)->register_callback(callback_sub); +} + +// remove item from list of work items +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + static_cast(node_handle)->unregister_callback(callback_sub); +} + +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + if (node_handle) { + return static_cast(node_handle)->get_instance(); + } + + return -1; +} + +/* These are optimized by inlining in NuttX Flat build */ +#if !defined(CONFIG_BUILD_FLAT) +unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) +{ + return is_advertised(node_handle) ? static_cast(node_handle)->updates_available( + last_generation) : 0; +} + +bool uORB::Manager::is_advertised(const void *node_handle) +{ + return static_cast(node_handle)->is_advertised(); +} +#endif + int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) { char path[orb_maxpath]; diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index b6e0ae784f14..682e6047efac 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -34,9 +34,11 @@ #ifndef _uORBManager_hpp_ #define _uORBManager_hpp_ +#include "uORBDeviceNode.hpp" #include "uORBCommon.hpp" #include "uORBDeviceMaster.hpp" +#include // For ORB_ID enum #include #ifdef __PX4_NUTTX @@ -54,8 +56,110 @@ namespace uORB { class Manager; +class SubscriptionCallback; } + +/* + * IOCTLs for manager to access device nodes using + * a handle + */ + +#define _ORBIOCDEV(_n) (_PX4_IOC(_ORBIOCDEVBASE, _n)) +#define ORBIOCDEVEXISTS _ORBIOCDEV(30) +typedef struct orbiocdevexists { + const ORB_ID orb_id; + const uint8_t instance; + const bool check_advertised; + int ret; +} orbiocdevexists_t; + +#define ORBIOCDEVADVERTISE _ORBIOCDEV(31) +typedef struct orbiocadvertise { + const struct orb_metadata *meta; + bool is_advertiser; + int *instance; + int ret; +} orbiocdevadvertise_t; + +#define ORBIOCDEVUNADVERTISE _ORBIOCDEV(32) +typedef struct orbiocunadvertise { + void *handle; + int ret; +} orbiocdevunadvertise_t; + +#define ORBIOCDEVPUBLISH _ORBIOCDEV(33) +typedef struct orbiocpublish { + const struct orb_metadata *meta; + orb_advert_t handle; + const void *data; + int ret; +} orbiocdevpublish_t; + +#define ORBIOCDEVADDSUBSCRIBER _ORBIOCDEV(34) +typedef struct { + const ORB_ID orb_id; + const uint8_t instance; + unsigned *initial_generation; + void *handle; +} orbiocdevaddsubscriber_t; + +#define ORBIOCDEVREMSUBSCRIBER _ORBIOCDEV(35) + +#define ORBIOCDEVQUEUESIZE _ORBIOCDEV(36) +typedef struct { + const void *handle; + uint8_t size; +} orbiocdevqueuesize_t; + +#define ORBIOCDEVDATACOPY _ORBIOCDEV(37) +typedef struct { + void *handle; + void *dst; + unsigned generation; + bool only_if_updated; + bool ret; +} orbiocdevdatacopy_t; + +#define ORBIOCDEVREGCALLBACK _ORBIOCDEV(38) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; + bool registered; +} orbiocdevregcallback_t; + +#define ORBIOCDEVUNREGCALLBACK _ORBIOCDEV(39) +typedef struct { + void *handle; + class uORB::SubscriptionCallback *callback_sub; +} orbiocdevunregcallback_t; + +#define ORBIOCDEVGETINSTANCE _ORBIOCDEV(40) +typedef struct { + const void *handle; + uint8_t instance; +} orbiocdevgetinstance_t; + +#define ORBIOCDEVUPDATESAVAIL _ORBIOCDEV(41) +typedef struct { + const void *handle; + unsigned last_generation; + unsigned ret; +} orbiocdevupdatesavail_t; + +#define ORBIOCDEVISADVERTISED _ORBIOCDEV(42) +typedef struct { + const void *handle; + bool ret; +} orbiocdevisadvertised_t; + +typedef enum { + ORB_DEVMASTER_STATUS = 0, + ORB_DEVMASTER_TOP = 1 +} orbiocdevmastercmd_t; +#define ORBIOCDEVMASTERCMD _ORBIOCDEV(45) + + /** * This is implemented as a singleton. This class manages creating the * uORB nodes for each uORB topics and also implements the behavor of the @@ -96,6 +200,10 @@ class uORB::Manager */ uORB::DeviceMaster *get_device_master(); +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + static int orb_ioctl(unsigned int cmd, unsigned long arg); +#endif + // ==== uORB interface methods ==== /** * Advertise as the publisher of a topic. @@ -150,11 +258,11 @@ class uORB::Manager * created instance, ie. 0 for the first advertiser, 1 for the next and so on. * @param queue_size Maximum number of buffered elements. If this is 1, no queuing is * used. - * @return PX4_ERROR on error, otherwise returns a handle + * @return nullptr on error, otherwise returns a handle * that can be used to publish to the topic. * If the topic in question is not known (due to an * ORB_DEFINE with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. + * this function will return nullptr and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, unsigned int queue_size = 1); @@ -165,7 +273,7 @@ class uORB::Manager * @param handle handle returned by orb_advertise or orb_advertise_multi. * @return 0 on success */ - int orb_unadvertise(orb_advert_t handle); + static int orb_unadvertise(orb_advert_t handle); /** * Publish new data to a topic. @@ -180,7 +288,7 @@ class uORB::Manager * @param data A pointer to the data to be published. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); + static int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); /** * Subscribe to a topic. @@ -301,7 +409,7 @@ class uORB::Manager * @param instance ORB instance * @return OK if the topic exists, PX4_ERROR otherwise. */ - int orb_exists(const struct orb_metadata *meta, int instance); + static int orb_exists(const struct orb_metadata *meta, int instance); /** * Set the minimum interval between which updates are seen for a subscription. @@ -335,6 +443,34 @@ class uORB::Manager */ int orb_get_interval(int handle, unsigned *interval); + static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance); + + static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation); + + static void orb_remove_internal_subscriber(void *node_handle); + + static uint8_t orb_get_queue_size(const void *node_handle); + + static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated); + + static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub); + + static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub); + + static uint8_t orb_get_instance(const void *node_handle); + +#if defined(CONFIG_BUILD_FLAT) + /* These are optimized by inlining in NuttX Flat build */ + static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast(node_handle)->updates_available(last_generation) : 0; } + + static bool is_advertised(const void *node_handle) { return static_cast(node_handle)->is_advertised(); } + +#else + static unsigned updates_available(const void *node_handle, unsigned last_generation); + + static bool is_advertised(const void *node_handle); +#endif + #ifdef ORB_COMMUNICATOR /** * Method to set the uORBCommunicator::IChannel instance. diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp new file mode 100644 index 000000000000..af00dbb9ba58 --- /dev/null +++ b/platforms/common/uORB/uORBManagerUsr.cpp @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 + +#include +#include +#include + +#include + +#include "uORBDeviceNode.hpp" +#include "uORBUtils.hpp" +#include "uORBManager.hpp" + +uORB::Manager *uORB::Manager::_Instance = nullptr; + +bool uORB::Manager::initialize() +{ + if (_Instance == nullptr) { + _Instance = new uORB::Manager(); + } + + return _Instance != nullptr; +} + +bool uORB::Manager::terminate() +{ + if (_Instance != nullptr) { + delete _Instance; + _Instance = nullptr; + return true; + } + + return false; +} + +uORB::Manager::Manager() +{ +} + +uORB::Manager::~Manager() +{ +} + +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ + // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { + return PX4_ERROR; + } + + orbiocdevexists_t data = {static_cast(meta->o_id), static_cast(instance), true, PX4_ERROR}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret; +} + +orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, + unsigned int queue_size) +{ + /* open the node as an advertiser */ + int fd = node_open(meta, true, instance); + + if (fd == PX4_ERROR) { + PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); + return nullptr; + } + + /* Set the queue size. This must be done before the first publication; thus it fails if + * this is not the first advertiser. + */ + int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + + if (result < 0 && queue_size > 1) { + PX4_WARN("orb_advertise_multi: failed to set queue size"); + } + + /* get the advertiser handle and close the node */ + orb_advert_t advertiser; + + result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + px4_close(fd); + + if (result == PX4_ERROR) { + PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); + return nullptr; + } + + /* the advertiser may perform an initial publish to initialise the object */ + if (data != nullptr) { + result = orb_publish(meta, advertiser, data); + + if (result == PX4_ERROR) { + PX4_ERR("orb_publish failed %s", meta->o_name); + return nullptr; + } + } + + return advertiser; +} + +int uORB::Manager::orb_unadvertise(orb_advert_t handle) +{ + orbiocdevunadvertise_t data = {handle, PX4_ERROR}; + boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast(&data)); + + return data.ret; +} + +int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) +{ + return node_open(meta, false); +} + +int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(meta, false, &inst); +} + +int uORB::Manager::orb_unsubscribe(int fd) +{ + return px4_close(fd); +} + +int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ + orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR}; + boardctl(ORBIOCDEVPUBLISH, reinterpret_cast(&d)); + + return d.ret; +} + +int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +{ + int ret; + + ret = px4_read(handle, buffer, meta->o_size); + + if (ret < 0) { + return PX4_ERROR; + } + + if (ret != (int)meta->o_size) { + errno = EIO; + return PX4_ERROR; + } + + return PX4_OK; +} + +int uORB::Manager::orb_check(int handle, bool *updated) +{ + /* Set to false here so that if `px4_ioctl` fails to false. */ + *updated = false; + return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); +} + +int uORB::Manager::orb_set_interval(int handle, unsigned interval) +{ + return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); +} + +int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +{ + int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); + *interval /= 1000; + return ret; +} + +int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) +{ + char path[orb_maxpath]; + int fd = -1; + int ret = PX4_ERROR; + + /* + * If meta is null, the object was not defined, i.e. it is not + * known to the system. We can't advertise/subscribe such a thing. + */ + if (nullptr == meta) { + errno = ENOENT; + return PX4_ERROR; + } + + /* if we have an instance and are an advertiser, we will generate a new node and set the instance, + * so we do not need to open here */ + if (!instance || !advertiser) { + /* + * Generate the path to the node and try to open it. + */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + if (ret != OK) { + errno = -ret; + return PX4_ERROR; + } + + /* open the path as either the advertiser or the subscriber */ + fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + *instance = 0; + } + + /* we may need to advertise the node... */ + if (fd < 0) { + ret = PX4_ERROR; + + orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR}; + boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data); + ret = data.ret; + + /* it's OK if it already exists */ + if ((ret != PX4_OK) && (EEXIST == errno)) { + ret = PX4_OK; + } + + if (ret == PX4_OK) { + /* update the path, as it might have been updated during the node advertise call */ + ret = uORB::Utils::node_mkpath(path, meta, instance); + + /* on success, try to open again */ + if (ret == PX4_OK) { + fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + + } else { + errno = -ret; + return PX4_ERROR; + } + } + } + + if (fd < 0) { + errno = EIO; + return PX4_ERROR; + } + + /* everything has been OK, we can return the handle now */ + return fd; +} + +bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +{ + orbiocdevexists_t data = {orb_id, instance, false, 0}; + boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); + + return data.ret == PX4_OK ? true : false; +} + +void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) +{ + orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr}; + boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast(&data)); + + return data.handle; +} + +void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) +{ + boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast(node_handle)); +} + +uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) +{ + orbiocdevqueuesize_t data = {node_handle, 0}; + boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast(&data)); + + return data.size; +} + +bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) +{ + orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false}; + boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); + generation = data.generation; + + return data.ret; +} + +bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevregcallback_t data = {node_handle, callback_sub, false}; + boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast(&data)); + + return data.registered; +} + +void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) +{ + orbiocdevunregcallback_t data = {node_handle, callback_sub}; + boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast(&data)); +} + +uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +{ + orbiocdevgetinstance_t data = {node_handle, 0}; + boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast(&data)); + + return data.instance; +} + +unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) +{ + orbiocdevupdatesavail_t data = {node_handle, last_generation, 0}; + boardctl(ORBIOCDEVUPDATESAVAIL, reinterpret_cast(&data)); + return data.ret; +} + +bool uORB::Manager::is_advertised(const void *node_handle) +{ + orbiocdevisadvertised_t data = {node_handle, false}; + boardctl(ORBIOCDEVISADVERTISED, reinterpret_cast(&data)); + return data.ret; +} diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index 531808fcb7a9..bc7cda513297 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -83,7 +83,7 @@ int uORBTest::UnitTest::pubsublatency_main() if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(orb_test_medium), test_multi_sub, &t); - unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.timestamp); + unsigned elt = (unsigned)hrt_elapsed_time(&t.timestamp); latency_integral += elt; timings[i] = elt; @@ -796,7 +796,7 @@ int uORBTest::UnitTest::test_SubscriptionMulti() if (sub.get_last_generation() != last_gen + 1) { //return test_fail("sub %d generation should be %d + 1, but it's %d", sub_instance, last_gen, sub.get_last_generation()); - PX4_ERR("sub %d generation should be %d + 1, but it's %d", sub_instance, last_gen, sub.get_last_generation()); + PX4_WARN("sub %d generation should be %d + 1, but it's %d", sub_instance, last_gen, sub.get_last_generation()); } } } diff --git a/platforms/common/work_queue/hrt_queue.c b/platforms/common/work_queue/hrt_queue.c index 74a4a8e17bb2..98c96d4161c7 100644 --- a/platforms/common/work_queue/hrt_queue.c +++ b/platforms/common/work_queue/hrt_queue.c @@ -123,7 +123,7 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del work->qtime = hrt_absolute_time(); /* Time work queued */ //PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime); - dq_addlast((dq_entry_t *)work, &wqueue->q); + dq_addlast(&work->dq, &wqueue->q); if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */ #ifdef __PX4_QURT diff --git a/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c index 26a7e509ddc2..9465f0d9f21b 100644 --- a/platforms/common/work_queue/hrt_thread.c +++ b/platforms/common/work_queue/hrt_thread.c @@ -155,7 +155,7 @@ static void hrt_work_process() if (elapsed >= work->delay) { /* Remove the ready-to-execute work from the list */ - (void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q)); + (void)dq_rem((dq_entry_t *)&work->dq, &wqueue->q); //PX4_INFO("Dequeued work=%p", work); /* Extract the work description from the entry (in case the work diff --git a/platforms/common/work_queue/hrt_work_cancel.c b/platforms/common/work_queue/hrt_work_cancel.c index e5f9f5eabf7a..2fdea7ee9e64 100644 --- a/platforms/common/work_queue/hrt_work_cancel.c +++ b/platforms/common/work_queue/hrt_work_cancel.c @@ -103,7 +103,7 @@ void hrt_work_cancel(struct work_s *work) * mark as availalbe (i.e., the worker field is nullified). */ - dq_rem((dq_entry_t *)work, &wqueue->q); + dq_rem(&work->dq, &wqueue->q); work->worker = NULL; } diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 62fdbe3c5762..d618d3ccd555 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -37,19 +37,34 @@ endif() include(cygwin_cygpath) -set(NUTTX_DIR ${PX4_BINARY_DIR}/NuttX/nuttx) -set(NUTTX_APPS_DIR ${PX4_BINARY_DIR}/NuttX/apps) - add_executable(px4 ${PX4_SOURCE_DIR}/platforms/common/empty.c) set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) set_target_properties(px4 PROPERTIES OUTPUT_NAME ${FW_NAME}) -add_dependencies(px4 git_nuttx nuttx_build) +add_dependencies(px4 git_nuttx) get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) +if (NOT CONFIG_BUILD_FLAT) + add_executable(px4_kernel ${PX4_SOURCE_DIR}/platforms/common/empty.c) + set(KERNEL_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}_kernel.elf) + set_target_properties(px4_kernel PROPERTIES OUTPUT_NAME ${KERNEL_NAME}) + add_dependencies(px4_kernel git_nuttx) + get_property(kernel_module_libraries GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES) +endif() + # build NuttX add_subdirectory(NuttX ${PX4_BINARY_DIR}/NuttX) +# check that CONFIG_ARCH_BOARD_CUSTOM_DIR is in PX4_BOARD_DIR +if(CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH) + get_filename_component(nuttx_defconfig_root "${NUTTX_DEFCONFIG}/../.." ABSOLUTE) + get_filename_component(nuttx_config_from_defconfig "${NUTTX_DIR}/${CONFIG_ARCH_BOARD_CUSTOM_DIR}" ABSOLUTE) + + if(NOT ${nuttx_defconfig_root} MATCHES ${nuttx_config_from_defconfig}) + message(FATAL_ERROR "NuttX custom board directory (${CONFIG_ARCH_BOARD_CUSTOM_DIR}) isn't in board directory (${PX4_BOARD_DIR})") + endif() +endif() + set(nuttx_libs) set(SCRIPT_PREFIX) if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") @@ -72,6 +87,7 @@ elseif("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") -Wl,-wrap,nxsched_process_timer -Wl,-wrap,nxsem_post -Wl,-wrap,nxsem_wait + -Wl,-wrap,nxsem_wait_uninterruptible -Wl,-wrap,arm_svcall -Wl,-wrap,nx_start -Wl,-wrap,exit @@ -83,19 +99,33 @@ else() endif() list(APPEND nuttx_libs - nuttx_apps - nuttx_arch - nuttx_binfmt - nuttx_c nuttx_boards - nuttx_xx nuttx_drivers nuttx_fs - nuttx_mm nuttx_sched -) + nuttx_crypto + nuttx_binfmt + nuttx_xx + ) -if (CONFIG_NET) +if (NOT CONFIG_BUILD_FLAT) + list(APPEND nuttx_libs + px4_board_ctrl + nuttx_karch + nuttx_kmm + nuttx_stubs + nuttx_kc + ) +else() + list(APPEND nuttx_libs + nuttx_apps + nuttx_arch + nuttx_mm + nuttx_c + ) +endif() + +if(CONFIG_NET) list(APPEND nuttx_libs nuttx_net) target_link_libraries(nuttx_fs INTERFACE nuttx_net) endif() @@ -104,66 +134,168 @@ file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_D # only in the cygwin environment: convert absolute linker script path to mixed windows (C:/...) # because even relative linker script paths are different for linux, mac and windows -CYGPATH(PX4_BINARY_DIR PX4_BINARY_DIR_CYG) +CYGPATH(NUTTX_CONFIG_DIR NUTTX_CONFIG_DIR_CYG) -target_link_libraries(nuttx_arch - INTERFACE - drivers_board - arch_hrt -) +if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external")) + set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_CONFIG}_unsigned.bin) -target_link_libraries(nuttx_c INTERFACE nuttx_drivers) -target_link_libraries(nuttx_xx INTERFACE nuttx_c) + add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_CONFIG}.bin + COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + DEPENDS ${PX4_BINARY_OUTPUT} + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + ) +else() + set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_CONFIG}.bin) +endif() -target_link_libraries(px4 PRIVATE +if (NOT CONFIG_BUILD_FLAT) - -nostartfiles - -nodefaultlibs - -nostdlib - -nostdinc++ + target_link_libraries(nuttx_karch + INTERFACE + drivers_board + arch_hrt + ) - -fno-exceptions - -fno-rtti - -Wl,--script=${PX4_BINARY_DIR_CYG}/NuttX/nuttx-config/scripts/${SCRIPT_PREFIX}script.ld - -Wl,-Map=${PX4_CONFIG}.map - -Wl,--warn-common - -Wl,--gc-sections + target_link_libraries(px4_kernel PRIVATE - -Wl,--start-group - ${nuttx_libs} - -Wl,--end-group + -nostartfiles + -nodefaultlibs + -nostdlib + -nostdinc++ - m - gcc - ) + -fno-exceptions + -fno-rtti + + -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld + + -Wl,-Map=${PX4_CONFIG}_kernel.map + -Wl,--warn-common + -Wl,--gc-sections + + -Wl,--start-group + ${nuttx_libs} + ${kernel_module_libraries} + px4_work_queue # TODO, this shouldn't be needed here? + -Wl,--end-group + + m + gcc + ) + + if (config_romfs_root) + add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS) + target_link_libraries(px4_kernel PRIVATE romfs) + endif() + + target_link_libraries(px4_kernel PRIVATE -Wl,--print-memory-usage) + + set(nuttx_userspace) + + list(APPEND nuttx_userspace + drivers_userspace + nuttx_arch + nuttx_apps + nuttx_mm + nuttx_proxies + nuttx_c + nuttx_xx + ) + + target_link_libraries(nuttx_c INTERFACE nuttx_proxies) + + target_link_libraries(px4 PRIVATE + + -nostartfiles + -nodefaultlibs + -nostdlib + -nostdinc++ + + -fno-exceptions + -fno-rtti + + -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld + + -Wl,-Map=${PX4_CONFIG}.map + -Wl,--warn-common + -Wl,--gc-sections + + -Wl,--start-group + ${nuttx_userspace} + -Wl,--end-group + + m + gcc + ) -if(NOT USE_LD_GOLD) target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage) -endif() -target_link_libraries(px4 PRIVATE ${module_libraries}) + target_link_libraries(px4 PRIVATE + ${module_libraries} + ) -if (config_romfs_root) - add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS) - target_link_libraries(px4 PRIVATE romfs) -endif() + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${PX4_BOARD}_user.bin + COMMAND ${CMAKE_OBJCOPY} --gap-fill 0xFF --pad-to ${CONFIG_NUTTX_USERSPACE} -O binary ${PX4_BINARY_DIR_REL}/${KERNEL_NAME} ${PX4_BINARY_OUTPUT} + COMMAND cat ${PX4_BINARY_DIR_REL}/${PX4_BOARD}_user.bin >> ${PX4_BINARY_OUTPUT} -if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external")) - set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_BOARD}_unsigned.bin) + DEPENDS px4 px4_kernel + ) - add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin - COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_BOARD}.bin - DEPENDS ${PX4_BINARY_OUTPUT} - WORKING_DIRECTORY ${PX4_SOURCE_DIR} -) else() -set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin) -endif() -add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} - COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} - DEPENDS px4 -) + target_link_libraries(nuttx_c INTERFACE nuttx_sched) # nxsched_get_streams + + target_link_libraries(nuttx_arch + INTERFACE + drivers_board + arch_hrt + arch_board_reset + ) + + target_link_libraries(nuttx_c INTERFACE nuttx_drivers) + target_link_libraries(nuttx_drivers INTERFACE nuttx_c) + target_link_libraries(nuttx_xx INTERFACE nuttx_c) + target_link_libraries(nuttx_fs INTERFACE nuttx_c) + + target_link_libraries(px4 PRIVATE + + -nostartfiles + -nodefaultlibs + -nostdlib + -nostdinc++ + + -fno-exceptions + -fno-rtti + -Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld + -Wl,-Map=${PX4_CONFIG}.map + -Wl,--warn-common + -Wl,--gc-sections + + -Wl,--start-group + ${nuttx_libs} + -Wl,--end-group + + m + gcc + ) + + if(NOT USE_LD_GOLD) + target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage) + endif() + + target_link_libraries(px4 PRIVATE ${module_libraries}) + + if(config_romfs_root) + add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS) + target_link_libraries(px4 PRIVATE romfs) + endif() + + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} + DEPENDS px4 + ) + +endif() # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -178,9 +310,9 @@ if (TARGET parameters_xml AND TARGET airframes_xml) --git_identity ${PX4_SOURCE_DIR} --parameter_xml ${PX4_BINARY_DIR}/parameters.xml --airframe_xml ${PX4_BINARY_DIR}/airframes.xml - --image ${PX4_BINARY_DIR}/${PX4_BOARD}.bin > ${fw_package} + --image ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin > ${fw_package} DEPENDS - ${PX4_BINARY_DIR}/${PX4_BOARD}.bin + ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin airframes_xml parameters_xml COMMENT "Creating ${fw_package}" @@ -197,6 +329,14 @@ if (TARGET parameters_xml AND TARGET airframes_xml) endif() +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_custom_command(OUTPUT ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin + DEPENDS px4 + ) + add_custom_target(px4_bootloader_keep ALL DEPENDS ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin) +endif() + # print weak symbols add_custom_target(weak_symbols COMMAND ${CMAKE_NM} $ | ${GREP} " w " | cat @@ -205,6 +345,20 @@ add_custom_target(weak_symbols USES_TERMINAL ) +# generate bootloader.elf and copy to top level build directory +if(NOT ("${PX4_BOARD_LABEL}" STREQUAL "bootloader") AND (EXISTS "${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")) + add_custom_command( + OUTPUT ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf + COMMAND + ${CMAKE_OBJCOPY} -I binary -O elf32-little --change-section-address .data=0x08000000 + ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin + ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf + DEPENDS ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin + WORKING_DIRECTORY ${PX4_BINARY_DIR} + ) + add_custom_target(bootloader_elf DEPENDS ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf) +endif() + # debugger helpers configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.gdbinit) @@ -248,6 +402,9 @@ if(NOT NUTTX_DIR MATCHES "external") elseif(CONFIG_ARCH_CHIP_STM32F469I) set(DEBUG_DEVICE "STM32F469II") set(DEBUG_SVD_FILE "STM32F469.svd") + elseif(CONFIG_ARCH_CHIP_STM32F722RE) + set(DEBUG_DEVICE "STM32F722RE") + set(DEBUG_SVD_FILE "STM32F7x2.svd") elseif(CONFIG_ARCH_CHIP_STM32F745VG) set(DEBUG_DEVICE "STM32F745VG") set(DEBUG_SVD_FILE "STM32F7x5.svd") diff --git a/platforms/nuttx/Debug/PX4 b/platforms/nuttx/Debug/PX4 index ee7b3ec746a9..cc682edbe2ce 100644 --- a/platforms/nuttx/Debug/PX4 +++ b/platforms/nuttx/Debug/PX4 @@ -11,6 +11,9 @@ end document px4 . Various macros for working with the PX4 firmware. . +. dmesg +. Prints the dmesg output (if available). +. . perf . Prints the state of all performance counters. . @@ -18,6 +21,22 @@ document px4 end +define dmesg + if g_console_buffer + printf "PX4 dmesg:\n" + # TODO: respect head and tail + printf "%s\n", g_console_buffer._buffer + else + printf "PX4 dmesg unavailable\n" + end +end + +document dmesg +. dmesg +. Prints dmesg (if available). +end + + define _perf_print set $hdr = (struct perf_ctr_header *)$arg0 #printf "%p\n", $hdr @@ -35,7 +54,11 @@ define _perf_print # PC_INTERVAL if $hdr->type == 2 set $interval = (struct perf_ctr_interval *)$hdr - printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, ($interval->time_last - $interval->time_first) / $interval->event_count, $interval->time_least, $interval->time_most + if $interval->event_count == 0 + printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, 0, $interval->time_least, $interval->time_most + else + printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, ($interval->time_last - $interval->time_first) / $interval->event_count, $interval->time_least, $interval->time_most + end end end diff --git a/platforms/nuttx/Debug/flash_bootloader.jlink.in b/platforms/nuttx/Debug/flash_bootloader.jlink.in deleted file mode 100644 index f47e0cad53a0..000000000000 --- a/platforms/nuttx/Debug/flash_bootloader.jlink.in +++ /dev/null @@ -1,9 +0,0 @@ -Device @DEBUG_DEVICE@ -eoe 1 -si SWD -speed auto -r -h -loadbin @BOARD_BL_FIRMWARE_BIN@,0x08000000 -go -qc diff --git a/platforms/nuttx/Debug/jlink_debug_gdb.sh.in b/platforms/nuttx/Debug/jlink_debug_gdb.sh.in index 26bd2efec575..9f434a37801e 100755 --- a/platforms/nuttx/Debug/jlink_debug_gdb.sh.in +++ b/platforms/nuttx/Debug/jlink_debug_gdb.sh.in @@ -1,14 +1,29 @@ #! /bin/sh -set -o xtrace +if command -v gdb-multiarch &> /dev/null +then + GDB_CMD=$(command -v gdb-multiarch) + +elif command -v arm-none-eabi-gdb &> /dev/null +then + GDB_CMD=$(command -v arm-none-eabi-gdb) + +else + echo "gdb arm-none-eabi or multi-arch not found" + exit 1 +fi @JLinkGDBServerExe_PATH@ -device @DEBUG_DEVICE@ -select usb -endian little -if SWD -speed auto -ir -LocalhostOnly 1 -strict -vd -singlerun & -cd @PX4_BINARY_DIR@ && @CMAKE_GDB@ -silent -nh \ +cd @PX4_BINARY_DIR@ && ${GDB_CMD} -silent -nh \ -iex "set auto-load safe-path @PX4_BINARY_DIR@" \ + -ix=@PX4_SOURCE_DIR@/platforms/nuttx/NuttX/nuttx/tools/nuttx-gdbinit \ -ex "target remote localhost:2331" \ -ex "monitor reset 0" \ -ex "load" \ -ex "monitor reset 0" \ -ex "continue" \ @PX4_CONFIG@.elf + +# exit with status of last command +exit $? diff --git a/platforms/nuttx/Debug/jlink_gdb_backtrace.sh b/platforms/nuttx/Debug/jlink_gdb_backtrace.sh new file mode 100755 index 000000000000..493db53bbba2 --- /dev/null +++ b/platforms/nuttx/Debug/jlink_gdb_backtrace.sh @@ -0,0 +1,49 @@ +#! /bin/sh + +if command -v gdb-multiarch &> /dev/null +then + GDB_CMD=$(command -v gdb-multiarch) + +elif command -v arm-none-eabi-gdb &> /dev/null +then + GDB_CMD=$(command -v arm-none-eabi-gdb) + +else + echo "gdb arm-none-eabi or multi-arch not found" + exit 1 +fi + +file ${1} + +gdb_cmd_file=$(mktemp) + +cat >"${gdb_cmd_file}" < /dev/null +then + GDB_CMD=$(command -v gdb-multiarch) + +elif command -v arm-none-eabi-gdb &> /dev/null +then + GDB_CMD=$(command -v arm-none-eabi-gdb) + +else + echo "gdb arm-none-eabi or multi-arch not found" + exit 1 +fi + +file ${1} + +gdb_cmd_file=$(mktemp) + +cat >"${gdb_cmd_file}" < /dev/null || die "Install flamegraph.pl first" +if [ -z `which flamegraph.pl` ]; then + echo "Install flamegraph.pl first. Available from:" + echo -e "\thttps://github.com/brendangregg/FlameGraph" + exit 1 +fi # # Parsing the arguments. Read this section for usage info. @@ -45,6 +56,7 @@ elf= append=0 fgfontsize=10 fgwidth=1900 +gdbdev=`ls /dev/serial/by-id/*Black_Magic_Probe*-if00` for i in "$@" do @@ -70,6 +82,9 @@ do --fgwidth=*) fgwidth="${i#*=}" ;; + --gdbdev=*) + gdbdev="${i#*=}" + ;; *) usage ;; @@ -104,7 +119,7 @@ then then arm-none-eabi-gdb $elf --nx --quiet --batch \ -ex "set print asm-demangle on" \ - -ex "target extended /dev/ttyACM0" \ + -ex "target extended $gdbdev" \ -ex "monitor swdp_scan" \ -ex "attach 1" \ -ex bt \ @@ -114,7 +129,7 @@ then else arm-none-eabi-gdb $elf --nx --quiet --batch \ -ex "set print asm-demangle on" \ - -ex "target extended /dev/ttyACM0" \ + -ex "target extended $gdbdev" \ -ex "monitor swdp_scan" \ -ex "attach 1" \ -ex "source $root/Nuttx.py" \ @@ -137,7 +152,7 @@ fi # # Folding the stacks. # -[ -f $stacksfile ] || die "Where are the stack samples?" +[ -f $stacksfile ] || die "Where are the stack samples? Expected file not found: $stacksfile" cat << 'EOF' > /tmp/pmpn-folder.py # diff --git a/platforms/nuttx/Debug/upload_jlink_gdb.sh b/platforms/nuttx/Debug/upload_jlink_gdb.sh index 81e52da0acae..a4546b42471e 100755 --- a/platforms/nuttx/Debug/upload_jlink_gdb.sh +++ b/platforms/nuttx/Debug/upload_jlink_gdb.sh @@ -1,14 +1,42 @@ #! /bin/sh -set -o xtrace - -gdb-multiarch -silent -nx -batch \ - -ex "target remote localhost:2331" \ - -ex "monitor reset 0" \ - -ex "load" \ - -ex "monitor reset 0" \ - -ex "monitor go" \ - -ex "monitor sleep 3000" \ - -ex "disconnect" \ - -ex "quit" \ - ${1} +if command -v gdb-multiarch &> /dev/null +then + GDB_CMD=$(command -v gdb-multiarch) + +elif command -v arm-none-eabi-gdb &> /dev/null +then + GDB_CMD=$(command -v arm-none-eabi-gdb) + +else + echo "gdb arm-none-eabi or multi-arch not found" + exit 1 +fi + +file ${1} + +gdb_cmd_file=$(mktemp) + +cat >"${gdb_cmd_file}" < ${PX4_BINARY_DIR}/NuttX/nuttx/.git - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp - DEPENDS - git_nuttx - ${copy_nuttx_files} - COMMENT "Copying NuttX/nuttx to ${CP_DST}" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} -) - -############################################################################### -# NuttX apps: copy to build directory -############################################################################### -file(RELATIVE_PATH CP_SRC ${CMAKE_SOURCE_DIR} ${NUTTX_SRC_DIR}/apps) -file(RELATIVE_PATH CP_DST ${CMAKE_SOURCE_DIR} ${PX4_BINARY_DIR}/NuttX) - -# setup custom command to copy changes later -file(GLOB_RECURSE copy_apps_files LIST_DIRECTORIES false ${NUTTX_SRC_DIR}/apps/*) -add_custom_command( - OUTPUT ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp - COMMAND ${NUTTX_COPY_CMD} ${NUTTX_COPY_CMD_OPTS} ${CP_SRC} ${CP_DST} - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp - DEPENDS - git_nuttx_apps - ${copy_apps_files} - COMMENT "Copying NuttX/apps to ${CP_DST}" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} -) -set(APPS_DIR ${CMAKE_CURRENT_BINARY_DIR}/apps) - - -# For any dependencies of commands on files we need to create a target. -# Otherwise, if "Unix Makefiles" are used as the generator the commands are run in -# parallel on the different files which often can lead to races or redundancies -# in our build. -# A nice write-up can be found here: -# https://samthursfield.wordpress.com/2015/11/21/cmake-dependencies-between-targets-and-files-and-custom-commands/#custom-commands-and-parallel-make -add_custom_target(nuttx_copy_and_apps_target - DEPENDS - ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp - ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp -) - -# If the board provides a Kconfig Use it or create an empty one -if(EXISTS ${NUTTX_CONFIG_DIR}/Kconfig) - add_custom_command( - OUTPUT ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_CONFIG_DIR}/Kconfig ${NUTTX_DIR}/boards/dummy/Kconfig - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp - DEPENDS - nuttx_copy_and_apps_target ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp - ) -else() - add_custom_command( - OUTPUT ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp - COMMAND ${CMAKE_COMMAND} -E touch ${NUTTX_DIR}/boards/dummy/Kconfig - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp - DEPENDS - nuttx_copy_and_apps_target ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp - ) -endif() - -add_custom_target(nuttx_config_kconfig_target DEPENDS ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp) - -############################################################################### -# NuttX configure +# NuttX build ############################################################################### -# copy NuttX config directory -file(RELATIVE_PATH CP_SRC ${NUTTX_DIR} ${PX4_BOARD_DIR}/nuttx-config) -file(RELATIVE_PATH CP_DST ${NUTTX_DIR} ${PX4_BINARY_DIR}/NuttX) -add_custom_command( - OUTPUT ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp - COMMAND ${NUTTX_COPY_CMD} ${NUTTX_COPY_CMD_OPTS} ${CP_SRC} ${CP_DST} - COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/nuttx-config/drivers - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx-config/drivers/Kconfig - COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/nuttx-config/src - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_SRC_DIR}/nsh_romfsimg.h ${PX4_BINARY_DIR}/NuttX/nuttx-config/include/nsh_romfsimg.h - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp - DEPENDS - ${NUTTX_CONFIG_DIR}/include/board.h - ${NUTTX_CONFIG_DIR}/scripts/script.ld - ${NUTTX_SRC_DIR}/nsh_romfsimg.h - nuttx_config_kconfig_target ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp - WORKING_DIRECTORY ${NUTTX_DIR} - COMMENT "Copying NuttX config ${NUTTX_CONFIG}" -) -add_custom_target(nuttx_copy_config_dir_target DEPENDS ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp) - -if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 9.3) - set(CMAKE_C_COMP_DEP_FLAGS -Wno-stringop-truncation) -endif() - -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Make.defs.in ${NUTTX_DIR}/Make.defs) +configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/Make.defs.in ${CMAKE_CURRENT_BINARY_DIR}/nuttx/Make.defs) -# copy compressed PX4 defconfig into nuttx and inflate +# inflate .config add_custom_command( - OUTPUT - ${NUTTX_DIR}/.config - ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp + OUTPUT ${NUTTX_DIR}/.config + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/nuttx/Make.defs ${NUTTX_DIR}/Make.defs COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DEFCONFIG} ${NUTTX_DIR}/.config - COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/tools/px4_nuttx_make_olddefconfig.sh > nuttx_olddefconfig.log - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DEFCONFIG} ${NUTTX_DIR}/defconfig + COMMAND ${NUTTX_SRC_DIR}/tools/px4_nuttx_make_olddefconfig.sh > ${CMAKE_CURRENT_BINARY_DIR}/nuttx_olddefconfig.log + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/.config ${CMAKE_CURRENT_BINARY_DIR}/nuttx/.config DEPENDS - ${NUTTX_DIR}/Make.defs ${NUTTX_DEFCONFIG} - nuttx_copy_config_dir_target ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp - ${CMAKE_CURRENT_SOURCE_DIR}/tools/px4_nuttx_make_olddefconfig.sh + ${NUTTX_DIR}/defconfig + ${CMAKE_CURRENT_BINARY_DIR}/nuttx/Make.defs WORKING_DIRECTORY ${NUTTX_DIR} - COMMENT "Copying NuttX compressed config ${NUTTX_CONFIG} and inflating (make olddefconfig)" + #USES_TERMINAL ) -add_custom_target(nuttx_config_target DEPENDS ${NUTTX_DIR}/.config ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp) -############################################################################### -# NuttX build -############################################################################### - -# verbose build settings (V=1 or VERBOSE=1) -option(PX4_NUTTX_VERBOSE "PX4 NuttX verbose build" off) - -if(($ENV{V} MATCHES "1") OR ($ENV{VERBOSE} MATCHES "1")) - message(STATUS "NuttX verbose build enabled") - set(PX4_NUTTX_VERBOSE on) -endif() - -if(PX4_NUTTX_VERBOSE) - set(nuttx_build_options) - set(nuttx_build_uses_terminal "USES_TERMINAL") -else() - set(nuttx_build_options "--quiet") - set(nuttx_build_uses_terminal) -endif() - -# context +# context (.config -> include/nuttx/config.h, include/nuttx/version.h, dirlinks) add_custom_command( - OUTPUT - ${NUTTX_DIR}/include/nuttx/config.h - ${NUTTX_DIR}/include/nuttx/version.h - COMMAND - make ${nuttx_build_options} --no-print-directory CONFIG_ARCH_BOARD_CUSTOM=y pass1dep > nuttx_context.log + OUTPUT ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/arch/chip + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/nuttx/.config ${NUTTX_DIR}/.config + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/nuttx/Make.defs ${NUTTX_DIR}/Make.defs + COMMAND make --no-print-directory --silent clean_context + COMMAND make --no-print-directory --silent pass1dep > ${CMAKE_CURRENT_BINARY_DIR}/nuttx_context.log DEPENDS - ${NUTTX_DIR}/Make.defs - nuttx_config_target ${NUTTX_DIR}/.config ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp + ${CMAKE_CURRENT_BINARY_DIR}/nuttx/Make.defs + ${NUTTX_DIR}/.config WORKING_DIRECTORY ${NUTTX_DIR} - ${nuttx_build_uses_terminal} + #USES_TERMINAL ) -add_custom_target(nuttx_context DEPENDS ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h) - -# library of NuttX libraries -add_library(nuttx_build INTERFACE) +add_custom_target(nuttx_context DEPENDS ${NUTTX_DIR}/include/nuttx/config.h) -add_custom_target(px4_config_file_target DEPENDS ${PX4_CONFIG_FILE}) # builtins -set(nuttx_builtin_list) +set(builtin_apps_string) +set(builtin_apps_decl_string) if(CONFIG_NSH_LIBRARY) - # force builtins regeneration and apps rebuild if nuttx or px4 configuration have changed - add_custom_command(OUTPUT ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp - COMMAND find ${APPS_DIR}/builtin/registry -name px4_\*.bdat -delete - COMMAND find ${APPS_DIR}/builtin/registry -name px4_\*.pdat -delete - COMMAND rm -f ${APPS_DIR}/builtin/builtin_list.h - COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp - DEPENDS - nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h - px4_config_file_target ${PX4_CONFIG_FILE} - ) - - add_custom_target(builtins_clean_target DEPENDS ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp) - + list(SORT module_libraries) foreach(module ${module_libraries}) get_target_property(MAIN ${module} MAIN) get_target_property(STACK_MAIN ${module} STACK_MAIN) get_target_property(PRIORITY ${module} PRIORITY) if(MAIN) - add_custom_command(OUTPUT ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.bdat - COMMAND echo "{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main }," > ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.bdat - COMMAND ${CMAKE_COMMAND} -E touch ${APPS_DIR}/builtin/registry/.updated - DEPENDS - builtins_clean_target ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp - nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h - VERBATIM - ) - list(APPEND nuttx_builtin_list ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.bdat) - - add_custom_command(OUTPUT ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.pdat - COMMAND echo "int ${MAIN}_main(int argc, char *argv[]);" > ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.pdat - COMMAND ${CMAKE_COMMAND} -E touch ${APPS_DIR}/builtin/registry/.updated - DEPENDS - builtins_clean_target ${PX4_BINARY_DIR}/NuttX/builtins_clean.stamp - nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h - VERBATIM - ) - list(APPEND nuttx_builtin_list ${APPS_DIR}/builtin/registry/px4_${MAIN}_main.pdat) - + set(builtin_apps_string "${builtin_apps_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main },\n") + set(builtin_apps_decl_string "${builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") endif() endforeach() endif() -add_custom_target(nuttx_builtin_list_target DEPENDS ${nuttx_builtin_list}) +if (NOT CONFIG_BUILD_FLAT) + set(KERNEL_BUILTIN_DIR ${CMAKE_CURRENT_BINARY_DIR}/kernel_builtin) + set(kernel_builtin_apps_string) + set(kernel_builtin_apps_proxy_string) + set(kernel_builtin_apps_decl_string) + + list(SORT kernel_module_libraries) + foreach(module ${kernel_module_libraries}) + get_target_property(MAIN ${module} MAIN) + get_target_property(STACK_MAIN ${module} STACK_MAIN) + get_target_property(PRIORITY ${module} PRIORITY) + + if(MAIN) + set(kernel_builtin_apps_string "${kernel_builtin_apps_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main },\n") + set(kernel_builtin_apps_proxy_string "${kernel_builtin_apps_proxy_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, launch_kmod_main },\n") + set(kernel_builtin_apps_decl_string "${kernel_builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") + endif() + endforeach() + + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.bdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.bdat) + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.pdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.pdat) + + add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/kernel_builtin_list.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_proto.h + WORKING_DIRECTORY ${KERNEL_BUILTIN_DIR} + COMMAND ${CMAKE_COMMAND} -E remove -f kernel_builtin_list.h kernel_builtin_proto.h + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.bdat kernel_builtin_list.h + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.pdat kernel_builtin_proto.h + ) + + add_custom_target(px4_kernel_builtin_list_target DEPENDS ${KERNEL_BUILTIN_DIR}/kernel_builtin_list.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_proto.h) + +endif() # NOT CONFIG_BUILD_FLAT + +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4.bdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4.bdat) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4.pdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4.pdat) # APPS # libapps.a -add_custom_command(OUTPUT ${APPS_DIR}/libapps.a - COMMAND find ${APPS_DIR} -name \*.o -delete - COMMAND make ${nuttx_build_options} --no-print-directory TOPDIR="${NUTTX_DIR}" > nuttx_apps.log - DEPENDS - nuttx_builtin_list_target ${nuttx_builtin_list} - nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h +file(GLOB_RECURSE nuttx_apps_files LIST_DIRECTORIES false + ${APPS_DIR}/*.c + ${APPS_DIR}/*.h +) +add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/apps/libapps.a + COMMAND ${CMAKE_COMMAND} -E remove -f ${APPS_DIR}/libapps.a ${APPS_DIR}/builtin/builtin_list.h ${APPS_DIR}/builtin/builtin_proto.h + COMMAND find ${APPS_DIR} -type f -name \*.o -delete + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4.bdat ${APPS_DIR}/builtin/registry/px4.bdat + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${CMAKE_CURRENT_BINARY_DIR}/px4.pdat ${APPS_DIR}/builtin/registry/px4.pdat + COMMAND ${CMAKE_COMMAND} -E touch_nocreate ${APPS_DIR}/builtin/registry/.updated + COMMAND make --no-print-directory --silent TOPDIR="${NUTTX_DIR}" > ${CMAKE_CURRENT_BINARY_DIR}/nuttx_apps.log + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${APPS_DIR}/libapps.a ${CMAKE_CURRENT_BINARY_DIR}/apps/libapps.a + DEPENDS ${nuttx_apps_files} nuttx_context ${NUTTX_DIR}/include/nuttx/config.h WORKING_DIRECTORY ${APPS_DIR} - ${nuttx_build_uses_terminal} + #USES_TERMINAL ) -add_custom_target(nuttx_apps_build DEPENDS ${APPS_DIR}/libapps.a) +add_custom_target(nuttx_apps_build DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/apps/libapps.a) add_library(nuttx_apps STATIC IMPORTED GLOBAL) -set_property(TARGET nuttx_apps PROPERTY IMPORTED_LOCATION ${APPS_DIR}/libapps.a) -add_dependencies(nuttx_build nuttx_apps_build) -target_link_libraries(nuttx_build INTERFACE nuttx_apps) +set_property(TARGET nuttx_apps PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/apps/libapps.a) +add_dependencies(nuttx_apps nuttx_apps_build) # helper for all targets -function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra) - file(GLOB_RECURSE nuttx_lib_files - LIST_DIRECTORIES false - ${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*) - - add_custom_command(OUTPUT ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a - COMMAND find ${nuttx_lib_dir} -type f -name *.o -delete - COMMAND make -C ${nuttx_lib_dir} ${nuttx_build_options} --no-print-directory all TOPDIR=${NUTTX_DIR} KERNEL=${kernel} EXTRAFLAGS=${extra} +function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra target) + if (${target} STREQUAL all) + set(nuttx_lib_target all) + else() + set(nuttx_lib_target lib${target}.a) + endif() + + file(GLOB_RECURSE nuttx_lib_files LIST_DIRECTORIES false + ${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.h + ) + + add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a + COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a + COMMAND find ${nuttx_lib_dir} -type f -name \*.o -delete + COMMAND make -C ${nuttx_lib_dir} --no-print-directory --silent ${nuttx_lib_target} TOPDIR="${NUTTX_DIR}" KERNEL=${kernel} EXTRAFLAGS=${extra} + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a DEPENDS ${nuttx_lib_files} - nuttx_context ${NUTTX_DIR}/include/nuttx/config.h ${NUTTX_DIR}/include/nuttx/version.h + nuttx_context ${NUTTX_DIR}/include/nuttx/config.h WORKING_DIRECTORY ${NUTTX_DIR} - ${nuttx_build_uses_terminal} + #USES_TERMINAL ) - add_custom_target(nuttx_${nuttx_lib}_build DEPENDS ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a) + add_custom_target(nuttx_${nuttx_lib}_build DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a) + add_library(nuttx_${nuttx_lib} STATIC IMPORTED GLOBAL) - set_property(TARGET nuttx_${nuttx_lib} PROPERTY IMPORTED_LOCATION ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a) - add_dependencies(nuttx_build nuttx_${nuttx_lib}_build) - target_link_libraries(nuttx_build INTERFACE nuttx_${nuttx_lib}) + set_property(TARGET nuttx_${nuttx_lib} PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a) + add_dependencies(nuttx_${nuttx_lib} nuttx_${nuttx_lib}_build) endfunction() # add_nuttx_dir(NAME DIRECTORY KERNEL EXTRA) -add_nuttx_dir(arch arch/arm/src y -D__KERNEL__) -add_nuttx_dir(binfmt binfmt y -D__KERNEL__) -add_nuttx_dir(boards boards y -D__KERNEL__) -add_nuttx_dir(drivers drivers y -D__KERNEL__) -add_nuttx_dir(fs fs y -D__KERNEL__) -add_nuttx_dir(sched sched y -D__KERNEL__) -add_nuttx_dir(c libs/libc n "") -add_nuttx_dir(xx libs/libxx n "") -add_nuttx_dir(mm mm n "") +add_nuttx_dir(binfmt binfmt y -D__KERNEL__ all) +add_nuttx_dir(boards boards y -D__KERNEL__ all) +add_nuttx_dir(drivers drivers y -D__KERNEL__ all) +add_nuttx_dir(fs fs y -D__KERNEL__ all) +add_nuttx_dir(sched sched y -D__KERNEL__ all) +add_nuttx_dir(xx libs/libxx n "" all) +add_nuttx_dir(crypto crypto y -D__KERNEL__ all) + +if (NOT CONFIG_BUILD_FLAT) + add_nuttx_dir(arch arch/${CONFIG_ARCH}/src n "" arch) + add_dependencies(nuttx_arch_build nuttx_karch_build) # can't build these in parallel + add_nuttx_dir(karch arch/arm/src y -D__KERNEL__ karch) + add_nuttx_dir(c libs/libc n "" c) + add_dependencies(nuttx_c_build nuttx_kc_build) # can't build these in parallel + add_nuttx_dir(kc libs/libc y -D__KERNEL__ kc) + add_nuttx_dir(mm mm n "" mm) + add_dependencies(nuttx_mm_build nuttx_kmm_build) # can't build these in parallel + add_nuttx_dir(kmm mm y -D__KERNEL__ kmm) + add_nuttx_dir(proxies syscall n "" proxies) + add_dependencies(nuttx_proxies_build nuttx_stubs_build) # can't build these in parallel + add_nuttx_dir(stubs syscall y -D__KERNEL__ stubs) +else() + add_nuttx_dir(arch arch/${CONFIG_ARCH}/src y -D__KERNEL__ all) + add_nuttx_dir(c libs/libc n "" all) + add_nuttx_dir(mm mm n "" mm) +endif() if(CONFIG_NET) - add_nuttx_dir(net net y -D__KERNEL__) + add_nuttx_dir(net net y -D__KERNEL__ all) endif() ############################################################################### -# NuttX oldconfig +# NuttX oldconfig: Update a configuration using a provided .config as base add_custom_target(oldconfig_nuttx - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y oldconfig - DEPENDS nuttx_config_target ${NUTTX_DIR}/.config + COMMAND make --no-print-directory --silent oldconfig + DEPENDS ${NUTTX_DIR}/.config WORKING_DIRECTORY ${NUTTX_DIR} COMMENT "Running NuttX make oldconfig for ${NUTTX_CONFIG}" USES_TERMINAL @@ -336,7 +224,7 @@ add_custom_target(oldconfig_nuttx # NuttX oldconfig + savedefconfig back to PX4 add_custom_target(oldconfig - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y savedefconfig + COMMAND make --no-print-directory --silent savedefconfig COMMAND ${CMAKE_COMMAND} -E copy ${NUTTX_DIR}/defconfig ${NUTTX_DEFCONFIG} COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/.config DEPENDS oldconfig_nuttx @@ -346,10 +234,10 @@ add_custom_target(oldconfig ) ############################################################################### -# NuttX olddefconfig +# NuttX olddefconfig: same as oldconfig, but additionally update deps and sets new symbols to their default value add_custom_target(olddefconfig_nuttx - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y olddefconfig - DEPENDS nuttx_config_target ${NUTTX_DIR}/.config + COMMAND make --no-print-directory --silent olddefconfig + DEPENDS ${NUTTX_DIR}/.config WORKING_DIRECTORY ${NUTTX_DIR} COMMENT "Running NuttX make olddefconfig for ${NUTTX_CONFIG}" USES_TERMINAL @@ -357,7 +245,7 @@ add_custom_target(olddefconfig_nuttx # NuttX olddefconfig + savedefconfig back to PX4 add_custom_target(olddefconfig - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y savedefconfig + COMMAND make --no-print-directory --silent savedefconfig COMMAND ${CMAKE_COMMAND} -E copy ${NUTTX_DIR}/defconfig ${NUTTX_DEFCONFIG} COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/.config DEPENDS olddefconfig_nuttx @@ -369,16 +257,16 @@ add_custom_target(olddefconfig ############################################################################### # NuttX menuconfig add_custom_target(menuconfig_nuttx - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y menuconfig - DEPENDS nuttx_config_target ${NUTTX_DIR}/.config + COMMAND make --no-print-directory --silent menuconfig + DEPENDS ${NUTTX_DIR}/.config WORKING_DIRECTORY ${NUTTX_DIR} COMMENT "Running NuttX make menuconfig for ${NUTTX_CONFIG}" USES_TERMINAL - ) +) # NuttX menuconfig + savedefconfig back to PX4 add_custom_target(menuconfig - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y savedefconfig + COMMAND make --no-print-directory --silent savedefconfig COMMAND ${CMAKE_COMMAND} -E copy ${NUTTX_DIR}/defconfig ${NUTTX_DEFCONFIG} COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/.config DEPENDS menuconfig_nuttx @@ -390,16 +278,16 @@ add_custom_target(menuconfig ############################################################################### # NuttX qconfig add_custom_target(qconfig_nuttx - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y qconfig - DEPENDS nuttx_config_target ${NUTTX_DIR}/.config + COMMAND make --no-print-directory --silent qconfig + DEPENDS ${NUTTX_DIR}/.config WORKING_DIRECTORY ${NUTTX_DIR} COMMENT "Running NuttX make qconfig for ${NUTTX_CONFIG}" USES_TERMINAL - ) +) # NuttX qconfig + savedefconfig back to PX4 add_custom_target(qconfig - COMMAND make --no-print-directory --silent -C ${NUTTX_DIR} CONFIG_ARCH_BOARD_CUSTOM=y savedefconfig + COMMAND make --no-print-directory --silent savedefconfig COMMAND ${CMAKE_COMMAND} -E copy ${NUTTX_DIR}/defconfig ${NUTTX_DEFCONFIG} COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/.config DEPENDS qconfig_nuttx diff --git a/platforms/nuttx/NuttX/Make.defs.in b/platforms/nuttx/NuttX/Make.defs.in index 5299325b46f3..1e56ddff8f29 100644 --- a/platforms/nuttx/NuttX/Make.defs.in +++ b/platforms/nuttx/NuttX/Make.defs.in @@ -109,6 +109,7 @@ FLAGS = $(MAXOPTIMIZATION) -g2 \ -Wno-format-truncation \ -Wno-maybe-uninitialized \ -Wno-missing-field-initializers \ + -Wno-stringop-truncation \ -Wno-sign-compare \ -Wno-type-limits \ -Wno-unused-parameter \ @@ -116,6 +117,11 @@ FLAGS = $(MAXOPTIMIZATION) -g2 \ FLAGS += $(EXTRAFLAGS) +# force alignment +ifeq ($(CONFIG_BOARD_FORCE_ALIGNMENT),y) + FLAGS += -mno-unaligned-access +endif + # enable precise stack overflow tracking ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) FLAGS += -finstrument-functions -ffixed-r10 diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index 9be72aa9c351..6d9010f47e7b 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit 9be72aa9c351186d7f7e54aad32a45b7f2ea3ee0 +Subproject commit 6d9010f47e7b00d9d86bbdd4630c78cd8d57ba35 diff --git a/platforms/nuttx/NuttX/nsh_romfsimg.h b/platforms/nuttx/NuttX/include/nsh_romfsimg.h similarity index 100% rename from platforms/nuttx/NuttX/nsh_romfsimg.h rename to platforms/nuttx/NuttX/include/nsh_romfsimg.h diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 011fbb2ce150..91bece51afbe 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 011fbb2ce150e05532e04b2f63d93ee932b226fb +Subproject commit 91bece51afbe7da9db12e3695cdbb4f4bba4bc83 diff --git a/platforms/nuttx/NuttX/px4.bdat.in b/platforms/nuttx/NuttX/px4.bdat.in new file mode 100644 index 000000000000..376baca08330 --- /dev/null +++ b/platforms/nuttx/NuttX/px4.bdat.in @@ -0,0 +1,2 @@ +@builtin_apps_string@ +@kernel_builtin_apps_proxy_string@ diff --git a/platforms/nuttx/NuttX/px4.pdat.in b/platforms/nuttx/NuttX/px4.pdat.in new file mode 100644 index 000000000000..0b9b1cbcbbbd --- /dev/null +++ b/platforms/nuttx/NuttX/px4.pdat.in @@ -0,0 +1 @@ +@builtin_apps_decl_string@ diff --git a/platforms/nuttx/NuttX/px4_kernel.bdat.in b/platforms/nuttx/NuttX/px4_kernel.bdat.in new file mode 100644 index 000000000000..2061dab6e09f --- /dev/null +++ b/platforms/nuttx/NuttX/px4_kernel.bdat.in @@ -0,0 +1 @@ +@kernel_builtin_apps_string@ diff --git a/platforms/nuttx/NuttX/px4_kernel.pdat.in b/platforms/nuttx/NuttX/px4_kernel.pdat.in new file mode 100644 index 000000000000..ecdf5e7b9d63 --- /dev/null +++ b/platforms/nuttx/NuttX/px4_kernel.pdat.in @@ -0,0 +1 @@ +@kernel_builtin_apps_decl_string@ diff --git a/platforms/nuttx/NuttX/tools/kconfig-conf b/platforms/nuttx/NuttX/tools/kconfig-conf index 776755355c1f..94a6f67992c3 100755 --- a/platforms/nuttx/NuttX/tools/kconfig-conf +++ b/platforms/nuttx/NuttX/tools/kconfig-conf @@ -4,7 +4,6 @@ echo "DEBUG: kconfiglib kconfig-conf wrapper, arguments: ${@}" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" export APPSDIR="`pwd`/../apps" -export CONFIG_ARCH_BOARD_CUSTOM=y if [ "command -v python3" ]; then PYTHON_EXECUTABLE=python3 diff --git a/platforms/nuttx/NuttX/tools/px4_nuttx_make_olddefconfig.sh b/platforms/nuttx/NuttX/tools/px4_nuttx_make_olddefconfig.sh index 3766d099badf..8dbcfc4c6898 100755 --- a/platforms/nuttx/NuttX/tools/px4_nuttx_make_olddefconfig.sh +++ b/platforms/nuttx/NuttX/tools/px4_nuttx_make_olddefconfig.sh @@ -5,4 +5,4 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" # update PATH to include kconfiglib scripts export PATH=${DIR}:${PATH} -make --no-print-directory --silent CONFIG_ARCH_BOARD_CUSTOM=y CONFIG_APPS_DIR="../apps" olddefconfig +make --no-print-directory --silent CONFIG_APPS_DIR="../apps" olddefconfig diff --git a/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake b/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake new file mode 100644 index 000000000000..9f36102bb778 --- /dev/null +++ b/platforms/nuttx/cmake/Platform/Generic-arm-none-eabi-gcc-cortex-m0plus.cmake @@ -0,0 +1,5 @@ +set(cpu_flags "-mcpu=cortex-m0plus -mthumb -mfloat-abi=soft") # Nuttx toochain uses -mfloat-abi=soft + +set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) +set(CMAKE_CXX_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) +set(CMAKE_ASM_FLAGS "${cpu_flags} -D__ASSEMBLY__" CACHE STRING "" FORCE) diff --git a/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake new file mode 100644 index 000000000000..94d37b084bea --- /dev/null +++ b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake @@ -0,0 +1,11 @@ + +if(CONFIG_ARCH_DPFPU) + message(STATUS "Enabling double FP precision hardware instructions") + set(cpu_flags "-march=rv64gc -mabi=lp64d -mcmodel=medany") +else() + set(cpu_flags "-march=rv64imac -mabi=lp64 -mcmodel=medany") +endif() + +set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) +set(CMAKE_CXX_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) +set(CMAKE_ASM_FLAGS "${cpu_flags} -D__ASSEMBLY__" CACHE STRING "" FORCE) diff --git a/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake new file mode 100644 index 000000000000..42f10bff332e --- /dev/null +++ b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake @@ -0,0 +1,44 @@ +# riscv64-unknown-elf toolchain + +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_VERSION 1) + +set(triple riscv64-unknown-elf) +set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) +set(TOOLCHAIN_PREFIX ${triple}) + +set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}-gcc) +set(CMAKE_C_COMPILER_TARGET ${triple}) + +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}-g++) +set(CMAKE_CXX_COMPILER_TARGET ${triple}) + +set(CMAKE_ASM_COMPILER ${TOOLCHAIN_PREFIX}-gcc) + +# needed for test compilation +set(CMAKE_EXE_LINKER_FLAGS_INIT "--specs=nosys.specs") + +# compiler tools +find_program(CMAKE_AR ${TOOLCHAIN_PREFIX}-gcc-ar) +find_program(CMAKE_GDB ${TOOLCHAIN_PREFIX}-gdb) +find_program(CMAKE_LD ${TOOLCHAIN_PREFIX}-ld) +find_program(CMAKE_LINKER ${TOOLCHAIN_PREFIX}-ld) +find_program(CMAKE_NM ${TOOLCHAIN_PREFIX}-gcc-nm) +find_program(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}-objcopy) +find_program(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}-objdump) +find_program(CMAKE_RANLIB ${TOOLCHAIN_PREFIX}-gcc-ranlib) +find_program(CMAKE_STRIP ${TOOLCHAIN_PREFIX}-strip) + +set(CMAKE_FIND_ROOT_PATH get_file_component(${CMAKE_C_COMPILER} PATH)) +set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) +set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# os tools +foreach(tool grep make) + string(TOUPPER ${tool} TOOL) + find_program(${TOOL} ${tool}) + if(NOT ${TOOL}) + message(FATAL_ERROR "could not find ${tool}") + endif() +endforeach() diff --git a/platforms/nuttx/cmake/init.cmake b/platforms/nuttx/cmake/init.cmake index 5d87ff837ddd..9245ed90f90a 100644 --- a/platforms/nuttx/cmake/init.cmake +++ b/platforms/nuttx/cmake/init.cmake @@ -56,113 +56,35 @@ set(NUTTX_CONFIG_DIR ${PX4_BOARD_DIR}/nuttx-config CACHE FILEPATH "PX4 NuttX con set(NUTTX_DEFCONFIG ${NUTTX_CONFIG_DIR}/${NUTTX_CONFIG}/defconfig CACHE FILEPATH "path to defconfig" FORCE) set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${NUTTX_DEFCONFIG}) -set(NUTTX_SRC_DIR ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX) -set(NUTTX_DIR ${PX4_BINARY_DIR}/NuttX/nuttx CACHE FILEPATH "NuttX directory" FORCE) -set(NUTTX_APPS_DIR ${PX4_BINARY_DIR}/NuttX/apps CACHE FILEPATH "NuttX apps directory" FORCE) +set(NUTTX_SRC_DIR ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX) +set(NUTTX_DIR ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx CACHE FILEPATH "NuttX directory" FORCE) +set(NUTTX_APPS_DIR ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/apps CACHE FILEPATH "NuttX apps directory" FORCE) px4_add_git_submodule(TARGET git_nuttx PATH "${NUTTX_SRC_DIR}/nuttx") px4_add_git_submodule(TARGET git_nuttx_apps PATH "${NUTTX_SRC_DIR}/apps") -if(CMAKE_HOST_APPLE OR CMAKE_HOST_WIN32) - # copy with rsync and create file dependencies - set(NUTTX_COPY_CMD "rsync") - set(NUTTX_COPY_CMD_OPTS) - list(APPEND NUTTX_COPY_CMD_OPTS - -rp - --inplace - ) -else() - # copy with hard links - # archive, recursive, force, link (hardlinks) - set(NUTTX_COPY_CMD "cp") - set(NUTTX_COPY_CMD_OPTS "-aRfl") -endif() - -execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX) - -############################################################################### -# NuttX: copy to build directory -############################################################################### -if((NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp) OR (NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx/Kconfig)) - execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/nuttx) - file(RELATIVE_PATH CP_SRC ${CMAKE_SOURCE_DIR} ${NUTTX_SRC_DIR}/nuttx) - file(RELATIVE_PATH CP_DST ${CMAKE_SOURCE_DIR} ${PX4_BINARY_DIR}/NuttX) - execute_process(COMMAND ${NUTTX_COPY_CMD} ${NUTTX_COPY_CMD_OPTS} ${CP_SRC} ${CP_DST} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) - - # replace NuttX .git with actual git repo location (absolute path) - execute_process( - COMMAND git rev-parse --absolute-git-dir - OUTPUT_VARIABLE nuttx_git_dir - WORKING_DIRECTORY ${NUTTX_SRC_DIR}/nuttx - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - execute_process(COMMAND ${CMAKE_COMMAND} -E remove ${PX4_BINARY_DIR}/NuttX/nuttx/.git) - file(WRITE ${PX4_BINARY_DIR}/NuttX/nuttx/.git "gitdir: ${nuttx_git_dir}") - - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_copy.stamp) -endif() - -############################################################################### -# NuttX apps: copy to build directory -############################################################################### -if((NOT EXISTS ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp) OR (NOT EXISTS ${PX4_BINARY_DIR}/NuttX/apps/Kconfig)) - execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/apps) - file(RELATIVE_PATH CP_SRC ${CMAKE_SOURCE_DIR} ${NUTTX_SRC_DIR}/apps) - file(RELATIVE_PATH CP_DST ${CMAKE_SOURCE_DIR} ${PX4_BINARY_DIR}/NuttX) - execute_process(COMMAND ${NUTTX_COPY_CMD} ${NUTTX_COPY_CMD_OPTS} ${CP_SRC} ${CP_DST} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/apps_copy.stamp) -endif() - -############################################################################### -# board nuttx-config -############################################################################### - -# If the board provides a Kconfig Use it or create an empty one -if((NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp) OR (NOT EXISTS ${NUTTX_DIR}/boards/dummy/Kconfig)) - if(EXISTS ${NUTTX_CONFIG_DIR}/Kconfig) - execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_CONFIG_DIR}/Kconfig ${NUTTX_DIR}/boards/dummy/Kconfig) - else() - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${NUTTX_DIR}/boards/dummy/Kconfig) - endif() - - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_config_kconfig.stamp) -endif() - -if((NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp) OR (NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx-config/drivers/Kconfig)) - # copy board's nuttx-config to NuttX/nuttx-config - file(RELATIVE_PATH CP_SRC ${CMAKE_SOURCE_DIR} ${PX4_BOARD_DIR}/nuttx-config) - file(RELATIVE_PATH CP_DST ${CMAKE_SOURCE_DIR} ${PX4_BINARY_DIR}/NuttX) - execute_process(COMMAND ${NUTTX_COPY_CMD} ${NUTTX_COPY_CMD_OPTS} ${CP_SRC} ${CP_DST} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) - - execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/nuttx-config/drivers) - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx-config/drivers/Kconfig) - execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/NuttX/nuttx-config/src) - execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_SRC_DIR}/nsh_romfsimg.h ${PX4_BINARY_DIR}/NuttX/nuttx-config/include/nsh_romfsimg.h) +# make olddefconfig (inflate defconfig to full .config) +execute_process(COMMAND ${CMAKE_COMMAND} -E make_directory ${NUTTX_CONFIG_DIR}/src) # needed for NuttX build +execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_SRC_DIR}/Make.defs.in ${NUTTX_DIR}/Make.defs) # Create a temporary Toplevel Make.defs for the oldconfig step +execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DEFCONFIG} ${NUTTX_DIR}/.config) +execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DEFCONFIG} ${NUTTX_DIR}/defconfig) - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_copy_config_dir.stamp) -endif() +set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${NUTTX_DIR}/defconfig) -# make olddefconfig (inflate defconfig to full .config) -if((NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp) OR (NOT EXISTS ${PX4_BINARY_DIR}/NuttX/nuttx/.config)) - execute_process(COMMAND ${CMAKE_COMMAND} -E copy ${NUTTX_SRC_DIR}/Make.defs.in ${NUTTX_DIR}/Make.defs) # Create a temporary Toplevel Make.defs for the oldconfig step - execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DEFCONFIG} ${NUTTX_DIR}/.config) - execute_process( - COMMAND ${NUTTX_SRC_DIR}/tools/px4_nuttx_make_olddefconfig.sh - WORKING_DIRECTORY ${NUTTX_DIR} - OUTPUT_FILE nuttx_olddefconfig.log - RESULT_VARIABLE ret - ) - execute_process(COMMAND ${CMAKE_COMMAND} -E touch ${PX4_BINARY_DIR}/NuttX/nuttx_olddefconfig.stamp) - # remove temporary top level Make.defs - execute_process(COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/Make.defs) -endif() +execute_process( + COMMAND ${NUTTX_SRC_DIR}/tools/px4_nuttx_make_olddefconfig.sh + WORKING_DIRECTORY ${NUTTX_DIR} + OUTPUT_FILE ${CMAKE_CURRENT_BINARY_DIR}/nuttx_olddefconfig.log + RESULT_VARIABLE ret +) +execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/.config ${PX4_BINARY_DIR}/NuttX/nuttx/.config) ############################################################################### # NuttX cmake defconfig ############################################################################### # parse nuttx config options for cmake -file(STRINGS ${NUTTX_DIR}/.config ConfigContents) +file(STRINGS ${PX4_BINARY_DIR}/NuttX/nuttx/.config ConfigContents) foreach(NameAndValue ${ConfigContents}) # Strip leading spaces string(REGEX REPLACE "^[ ]+" "" NameAndValue ${NameAndValue}) @@ -174,13 +96,11 @@ foreach(NameAndValue ${ConfigContents}) # Find the value string(REPLACE "${Name}=" "" Value ${NameAndValue}) - if(Value) - # remove extra quotes - string(REPLACE "\"" "" Value ${Value}) + # remove extra quotes + string(REPLACE "\"" "" Value ${Value}) - # Set the variable - #message(STATUS "${Name} ${Value}") - set(${Name} ${Value} CACHE INTERNAL "NUTTX DEFCONFIG: ${Name}" FORCE) - endif() + # Set the variable + #message(STATUS "${Name} ${Value}") + set(${Name} ${Value} CACHE INTERNAL "NUTTX DEFCONFIG: ${Name}" FORCE) endif() endforeach() diff --git a/platforms/nuttx/cmake/jlink.cmake b/platforms/nuttx/cmake/jlink.cmake index c8ab8ca1f21f..ef6e3ad11286 100644 --- a/platforms/nuttx/cmake/jlink.cmake +++ b/platforms/nuttx/cmake/jlink.cmake @@ -31,11 +31,11 @@ # ############################################################################ -# jlink_upload (flash binary) find_program(JLinkGDBServerCLExe_PATH JLinkGDBServerCLExe HINTS /Applications/SEGGER/JLink ) if(JLinkGDBServerCLExe_PATH) + # jlink_upload (flash binary) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/jlink_gdb_start.sh.in ${PX4_BINARY_DIR}/jlink_gdb_start.sh @ONLY) add_custom_target(jlink_upload COMMAND ${PX4_BINARY_DIR}/jlink_gdb_start.sh @@ -47,6 +47,48 @@ if(JLinkGDBServerCLExe_PATH) WORKING_DIRECTORY ${PX4_BINARY_DIR} USES_TERMINAL ) + + # jlink_gdb_backtrace (attach, print current tasks, back trace, exit) + add_custom_target(jlink_gdb_backtrace + COMMAND ${PX4_BINARY_DIR}/jlink_gdb_start.sh + COMMAND ${CMAKE_COMMAND} -E env WORKSPACE=${PX4_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/Debug/jlink_gdb_backtrace.sh $ + DEPENDS + px4 + ${PX4_BINARY_DIR}/jlink_gdb_start.sh + ${CMAKE_CURRENT_SOURCE_DIR}/Debug/jlink_gdb_backtrace.sh + WORKING_DIRECTORY ${PX4_BINARY_DIR} + USES_TERMINAL + ) + + # jlink_gdb_backtrace_simple (attach, print current tasks, back trace, exit) + add_custom_target(jlink_gdb_backtrace_simple + COMMAND ${PX4_BINARY_DIR}/jlink_gdb_start.sh + COMMAND ${CMAKE_COMMAND} -E env WORKSPACE=${PX4_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/Debug/jlink_gdb_backtrace_simple.sh $ + DEPENDS + px4 + ${PX4_BINARY_DIR}/jlink_gdb_start.sh + ${CMAKE_CURRENT_SOURCE_DIR}/Debug/jlink_gdb_backtrace_simple.sh + WORKING_DIRECTORY ${PX4_BINARY_DIR} + USES_TERMINAL + ) + + + # jlink_upload_bootloader + # board directory supplied bootloader.bin + if(TARGET bootloader_elf) + # jlink_upload_bootloader + add_custom_target(jlink_upload_bootloader + COMMAND ${PX4_BINARY_DIR}/jlink_gdb_start.sh + COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/Debug/upload_jlink_gdb.sh ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf + DEPENDS + ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf + ${PX4_BINARY_DIR}/jlink_gdb_start.sh + ${CMAKE_CURRENT_SOURCE_DIR}/Debug/upload_jlink_gdb.sh + WORKING_DIRECTORY ${PX4_BINARY_DIR} + USES_TERMINAL + ) + endif() + endif() # jlink_debug_gdb (flash binary and run with gdb attached) @@ -67,7 +109,7 @@ if(JLinkGDBServerExe_PATH AND CMAKE_GDB) endif() # jlink_debug_ozone (run Segger Ozone debugger with current target configuration) -find_program(Ozone_PATH Ozone +find_program(Ozone_PATH Ozone ozone HINTS /Applications/Ozone.app/Contents/MacOS/ ) if(Ozone_PATH) @@ -82,38 +124,36 @@ if(Ozone_PATH) ) endif() -if(bootloader_bin OR (EXISTS "${PX4_BOARD_DIR}/bootloader/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")) - # jlink_flash_bootloader - find_program(JLinkExe_PATH JLinkExe) - if(JLinkExe_PATH) +# .bin flashing +find_program(JLinkExe_PATH JLinkExe) +if(JLinkExe_PATH) + + # jlink_flash_bootloader_bin + if(EXISTS ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin) - if(bootloader_bin) - set(BOARD_BL_FIRMWARE_BIN ${bootloader_bin}) - else() - set(BOARD_BL_FIRMWARE_BIN ${PX4_BOARD_DIR}/bootloader/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin) - endif() + set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin") + set(BOARD_FIRMWARE_APP_OFFSET "0x08000000") + configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in ${PX4_BINARY_DIR}/flash_bootloader_bin.jlink @ONLY) - configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/flash_bootloader.jlink.in ${PX4_BINARY_DIR}/flash_bootloader.jlink @ONLY) - add_custom_target(jlink_flash_bootloader - COMMAND ${JLinkExe_PATH} -CommandFile ${PX4_BINARY_DIR}/flash_bootloader.jlink + add_custom_target(jlink_flash_bootloader_bin + ${CMAKE_COMMAND} -E copy_if_different ${PX4_BOARD_DIR}/extras/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin ${PX4_BINARY_DIR}/${BOARD_FIRMWARE_BIN} + COMMAND ${JLinkExe_PATH} -CommandFile ${PX4_BINARY_DIR}/flash_bootloader_bin.jlink DEPENDS - px4 - ${CMAKE_CURRENT_SOURCE_DIR}/Debug/flash_bootloader.jlink.in + ${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in WORKING_DIRECTORY ${PX4_BINARY_DIR} USES_TERMINAL ) + endif() -endif() -if(uavcan_bl_image_name) - # jlink_flash_bootloader - find_program(JLinkExe_PATH JLinkExe) - if(JLinkExe_PATH) - set(BOARD_FIRMWARE_BIN ${PX4_BINARY_DIR}/${uavcan_bl_image_name}) + # jlink_flash_bin + if(uavcan_bl_image_name) + # uavcan signed firmware + set(BOARD_FIRMWARE_BIN ${uavcan_bl_image_name}) set(BOARD_FIRMWARE_APP_OFFSET "0x08010000") - configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in ${PX4_BINARY_DIR}/flash_bin.jlink @ONLY) - add_custom_target(jlink_flash_uavcan_bin + + add_custom_target(jlink_flash_bin COMMAND ${JLinkExe_PATH} -CommandFile ${PX4_BINARY_DIR}/flash_bin.jlink DEPENDS ${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in @@ -121,5 +161,21 @@ if(uavcan_bl_image_name) WORKING_DIRECTORY ${PX4_BINARY_DIR} USES_TERMINAL ) + else() + # regular firmware ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + set(BOARD_FIRMWARE_BIN ${PX4_CONFIG}.bin) + set(BOARD_FIRMWARE_APP_OFFSET "0x08008000") # TODO: get from board + configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in ${PX4_BINARY_DIR}/flash_bin.jlink @ONLY) + + add_custom_target(jlink_flash_bin + COMMAND ${CMAKE_COMMAND} -E echo "WARNING jlink_flash_bin currently assumes starting address ${BOARD_FIRMWARE_APP_OFFSET}" + COMMAND ${JLinkExe_PATH} -CommandFile ${PX4_BINARY_DIR}/flash_bin.jlink + DEPENDS + ${PX4_SOURCE_DIR}/platforms/nuttx/Debug/flash_bin.jlink.in + ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + WORKING_DIRECTORY ${PX4_BINARY_DIR} + USES_TERMINAL + ) endif() + endif() diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 67ce2e770077..d68b4becb7e7 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -51,17 +51,17 @@ function(px4_os_add_flags) include_directories(BEFORE SYSTEM - ${PX4_BINARY_DIR}/NuttX/nuttx/include - ${PX4_BINARY_DIR}/NuttX/nuttx/include/cxx + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/include + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/include/cxx ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/include/cxx # custom new ) include_directories( - ${PX4_BINARY_DIR}/NuttX/nuttx/arch/${CONFIG_ARCH}/src/${CONFIG_ARCH_FAMILY} - ${PX4_BINARY_DIR}/NuttX/nuttx/arch/${CONFIG_ARCH}/src/chip - ${PX4_BINARY_DIR}/NuttX/nuttx/arch/${CONFIG_ARCH}/src/common + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/${CONFIG_ARCH}/src/${CONFIG_ARCH_FAMILY} + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/${CONFIG_ARCH}/src/chip + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/${CONFIG_ARCH}/src/common - ${PX4_BINARY_DIR}/NuttX/apps/include + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/apps/include ) # prevent using the toolchain's std c++ library @@ -82,7 +82,14 @@ function(px4_os_add_flags) -ffixed-r10 -finstrument-functions # instrumenting PX4 Matrix and Param methods is too burdensome - -finstrument-functions-exclude-file-list=matrix/Matrix.hpp,px4_platform_common/param.h + -finstrument-functions-exclude-file-list=matrix/Matrix.hpp,px4_platform_common/param.h,modules__ekf2_unity.cpp + ) + endif() + + if("${CONFIG_BOARD_FORCE_ALIGNMENT}" STREQUAL "y") + message(STATUS "Board forcing alignment") + add_compile_options( + -mno-unaligned-access ) endif() @@ -124,6 +131,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_S32K146) set(CHIP_MANUFACTURER "nxp") set(CHIP "s32k14x") + elseif(CONFIG_ARCH_CHIP_RP2040) + set(CHIP_MANUFACTURER "rpi") + set(CHIP "rp2040") else() message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.") endif() @@ -167,7 +177,7 @@ function(px4_os_prebuild_targets) endif() add_library(prebuild_targets INTERFACE) - target_link_libraries(prebuild_targets INTERFACE nuttx_xx nuttx_c nuttx_fs nuttx_mm nuttx_sched m gcc) - add_dependencies(prebuild_targets DEPENDS nuttx_build uorb_headers) + target_link_libraries(prebuild_targets INTERFACE nuttx_xx m gcc) + add_dependencies(prebuild_targets DEPENDS nuttx_context uorb_headers) endfunction() diff --git a/platforms/nuttx/init/imxrt/rc.board_arch_defaults b/platforms/nuttx/init/imxrt/rc.board_arch_defaults index 588022d65405..d1dd00ad9aaf 100644 --- a/platforms/nuttx/init/imxrt/rc.board_arch_defaults +++ b/platforms/nuttx/init/imxrt/rc.board_arch_defaults @@ -3,4 +3,6 @@ # imxrt specific defaults #------------------------------------------------------------------------------ +param set-default -s MC_AT_EN 1 + set LOGGER_BUF 32 diff --git a/platforms/nuttx/init/kinetis/rc.board_arch_defaults b/platforms/nuttx/init/kinetis/rc.board_arch_defaults index d13ffa2fdc2c..db787a64a32b 100644 --- a/platforms/nuttx/init/kinetis/rc.board_arch_defaults +++ b/platforms/nuttx/init/kinetis/rc.board_arch_defaults @@ -9,10 +9,10 @@ param set-default SENS_IMU_MODE 1 param set-default EKF2_MULTI_MAG 0 param set-default SENS_MAG_MODE 1 -set LOGGER_BUF 12 +set LOGGER_BUF 8 if param greater -s UAVCAN_ENABLE 1 then # Reduce logger buffer to free up some RAM for UAVCAN servers. - set LOGGER_BUF 4 + set LOGGER_BUF 6 fi diff --git a/platforms/nuttx/init/rc.board_bootloader_upgrade.in b/platforms/nuttx/init/rc.board_bootloader_upgrade.in new file mode 100644 index 000000000000..23ea208507cf --- /dev/null +++ b/platforms/nuttx/init/rc.board_bootloader_upgrade.in @@ -0,0 +1,17 @@ +#! /bin/sh + +# +# Bootloader upgrade +# +if param compare -s SYS_BL_UPDATE 1 +then + if [ -f "/etc/extras/@BOARD_FIRMWARE_BIN@" ] + then + param set SYS_BL_UPDATE 0 + param save + echo "bootloader update..." + bl_update "/etc/extras/@BOARD_FIRMWARE_BIN@" + echo "bootloader update done, rebooting" + reboot + fi +fi diff --git a/platforms/nuttx/init/s32k1xx/rc.board_arch_defaults b/platforms/nuttx/init/s32k1xx/rc.board_arch_defaults index 9342373d4895..0d409fb63270 100644 --- a/platforms/nuttx/init/s32k1xx/rc.board_arch_defaults +++ b/platforms/nuttx/init/s32k1xx/rc.board_arch_defaults @@ -3,10 +3,10 @@ # S32K1XX specific defaults #------------------------------------------------------------------------------ -set LOGGER_BUF 12 +set LOGGER_BUF 8 if param greater -s UAVCAN_ENABLE 1 then # Reduce logger buffer to free up some RAM for UAVCAN servers. - set LOGGER_BUF 4 + set LOGGER_BUF 6 fi diff --git a/platforms/nuttx/init/stm32/rc.board_arch_defaults b/platforms/nuttx/init/stm32/rc.board_arch_defaults index b584f9752648..6380c3047b26 100644 --- a/platforms/nuttx/init/stm32/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32/rc.board_arch_defaults @@ -9,10 +9,10 @@ param set-default SENS_IMU_MODE 1 param set-default EKF2_MULTI_MAG 0 param set-default SENS_MAG_MODE 1 -set LOGGER_BUF 12 +set LOGGER_BUF 8 if param greater -s UAVCAN_ENABLE 1 then # Reduce logger buffer to free up some RAM for UAVCAN servers. - set LOGGER_BUF 4 + set LOGGER_BUF 6 fi diff --git a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults index 63b2772eb55f..1a9249795041 100644 --- a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults @@ -9,8 +9,10 @@ param set-default SENS_IMU_MODE 0 param set-default EKF2_MULTI_MAG 0 param set-default SENS_MAG_MODE 1 -param set-default IMU_GYRO_FFT_EN 1 +param set-default -s IMU_GYRO_FFT_EN 1 -param set-default UAVCAN_ENABLE 2 +param set-default -s MC_AT_EN 1 + +param set-default -s UAVCAN_ENABLE 2 set LOGGER_BUF 64 diff --git a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults index 87658a832f0a..9d1fee504cf8 100644 --- a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults @@ -9,8 +9,10 @@ param set-default SENS_IMU_MODE 0 param set-default EKF2_MULTI_MAG 3 param set-default SENS_MAG_MODE 0 -param set-default IMU_GYRO_FFT_EN 1 +param set-default -s IMU_GYRO_FFT_EN 1 -param set-default UAVCAN_ENABLE 2 +param set-default -s MC_AT_EN 1 + +param set-default -s UAVCAN_ENABLE 2 set LOGGER_BUF 64 diff --git a/platforms/nuttx/src/bootloader/CMakeLists.txt b/platforms/nuttx/src/bootloader/CMakeLists.txt index ac329c7e78b9..558e7f338589 100644 --- a/platforms/nuttx/src/bootloader/CMakeLists.txt +++ b/platforms/nuttx/src/bootloader/CMakeLists.txt @@ -31,11 +31,7 @@ # ############################################################################ -add_library(bootloader - bl.c - ${PX4_CHIP}/main.c -) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/common) -include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(lib) -add_dependencies(bootloader prebuild_targets) +add_subdirectory(common) +add_subdirectory(${PX4_CHIP_MANUFACTURER}) diff --git a/platforms/nuttx/src/bootloader/common/CMakeLists.txt b/platforms/nuttx/src/bootloader/common/CMakeLists.txt new file mode 100644 index 000000000000..7ba4af38266d --- /dev/null +++ b/platforms/nuttx/src/bootloader/common/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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(bootloader + bl.c + image_toc.c + crypto.c +) + +target_link_libraries(bootloader + PRIVATE + arch_bootloader +) + +if (DEFINED PX4_CRYPTO) + target_link_libraries(bootloader + PRIVATE + crypto_backend + ) +endif() + +add_dependencies(bootloader prebuild_targets) + +add_subdirectory(lib) diff --git a/platforms/nuttx/src/bootloader/bl.c b/platforms/nuttx/src/bootloader/common/bl.c similarity index 76% rename from platforms/nuttx/src/bootloader/bl.c rename to platforms/nuttx/src/bootloader/common/bl.c index 9d14314d61c2..1e934ed5283a 100644 --- a/platforms/nuttx/src/bootloader/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -43,11 +43,18 @@ #include #include #include +#include #include "bl.h" +#include "image_toc.h" +#include "crypto.h" #include "cdcacm.h" #include "uart.h" +#ifdef BOOTLOADER_USE_SECURITY +#include +#endif + // bootloader flash update protocol. // // Command format: @@ -73,69 +80,69 @@ // RESET finalise flash programming, reset chip and starts application // -#define BL_PROTOCOL_VERSION 5 // The revision of the bootloader protocol +#define BL_PROTOCOL_VERSION 5 // The revision of the bootloader protocol //* Next revision needs to update // protocol bytes -#define PROTO_INSYNC 0x12 // 'in sync' byte sent before status -#define PROTO_EOC 0x20 // end of command +#define PROTO_INSYNC 0x12 // 'in sync' byte sent before status +#define PROTO_EOC 0x20 // end of command // Reply bytes -#define PROTO_OK 0x10 // INSYNC/OK - 'ok' response -#define PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response -#define PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands -#define PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon -#define PROTO_RESERVED_0X15 0x15 // Reserved +#define PROTO_OK 0x10 // INSYNC/OK - 'ok' response +#define PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response +#define PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands +#define PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon +#define PROTO_RESERVED_0X15 0x15 // Reserved // see https://pixhawk.org/help/errata // Command bytes -#define PROTO_GET_SYNC 0x21 // NOP for re-establishing sync -#define PROTO_GET_DEVICE 0x22 // get device ID bytes -#define PROTO_CHIP_ERASE 0x23 // erase program area and reset program address -#define PROTO_PROG_MULTI 0x27 // write bytes at program address and increment -#define PROTO_GET_CRC 0x29 // compute & return a CRC -#define PROTO_GET_OTP 0x2a // read a byte from OTP at the given address -#define PROTO_GET_SN 0x2b // read a word from UDID area ( Serial) at the given address -#define PROTO_GET_CHIP 0x2c // read chip version (MCU IDCODE) -#define PROTO_SET_DELAY 0x2d // set minimum boot delay -#define PROTO_GET_CHIP_DES 0x2e // read chip version In ASCII -#define PROTO_BOOT 0x30 // boot the application -#define PROTO_DEBUG 0x31 // emit debug information - format not defined -#define PROTO_SET_BAUD 0x33 // set baud rate on uart - -#define PROTO_RESERVED_0X36 0x36 // Reserved -#define PROTO_RESERVED_0X37 0x37 // Reserved -#define PROTO_RESERVED_0X38 0x38 // Reserved -#define PROTO_RESERVED_0X39 0x39 // Reserved - -#define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size -#define PROTO_READ_MULTI_MAX 255 // size of the size field +#define PROTO_GET_SYNC 0x21 // NOP for re-establishing sync +#define PROTO_GET_DEVICE 0x22 // get device ID bytes +#define PROTO_CHIP_ERASE 0x23 // erase program area and reset program address +#define PROTO_PROG_MULTI 0x27 // write bytes at program address and increment +#define PROTO_GET_CRC 0x29 // compute & return a CRC +#define PROTO_GET_OTP 0x2a // read a byte from OTP at the given address +#define PROTO_GET_SN 0x2b // read a word from UDID area ( Serial) at the given address +#define PROTO_GET_CHIP 0x2c // read chip version (MCU IDCODE) +#define PROTO_SET_DELAY 0x2d // set minimum boot delay +#define PROTO_GET_CHIP_DES 0x2e // read chip version In ASCII +#define PROTO_BOOT 0x30 // boot the application +#define PROTO_DEBUG 0x31 // emit debug information - format not defined +#define PROTO_SET_BAUD 0x33 // set baud rate on uart + +#define PROTO_RESERVED_0X36 0x36 // Reserved +#define PROTO_RESERVED_0X37 0x37 // Reserved +#define PROTO_RESERVED_0X38 0x38 // Reserved +#define PROTO_RESERVED_0X39 0x39 // Reserved + +#define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size +#define PROTO_READ_MULTI_MAX 255 // size of the size field /* argument values for PROTO_GET_DEVICE */ -#define PROTO_DEVICE_BL_REV 1 // bootloader revision -#define PROTO_DEVICE_BOARD_ID 2 // board ID -#define PROTO_DEVICE_BOARD_REV 3 // board revision -#define PROTO_DEVICE_FW_SIZE 4 // size of flashable area -#define PROTO_DEVICE_VEC_AREA 5 // contents of reserved vectors 7-10 +#define PROTO_DEVICE_BL_REV 1 // bootloader revision +#define PROTO_DEVICE_BOARD_ID 2 // board ID +#define PROTO_DEVICE_BOARD_REV 3 // board revision +#define PROTO_DEVICE_FW_SIZE 4 // size of flashable area +#define PROTO_DEVICE_VEC_AREA 5 // contents of reserved vectors 7-10 -#define STATE_PROTO_OK 0x10 // INSYNC/OK - 'ok' response -#define STATE_PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response -#define STATE_PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands -#define STATE_PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon -#define STATE_PROTO_RESERVED_0X15 0x15 // Reserved +#define STATE_PROTO_OK 0x10 // INSYNC/OK - 'ok' response +#define STATE_PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response +#define STATE_PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands +#define STATE_PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon +#define STATE_PROTO_RESERVED_0X15 0x15 // Reserved // State -#define STATE_PROTO_GET_SYNC 0x1 // Have Seen NOP for re-establishing sync -#define STATE_PROTO_GET_DEVICE 0x2 // Have Seen get device ID bytes -#define STATE_PROTO_CHIP_ERASE 0x4 // Have Seen erase program area and reset program address -#define STATE_PROTO_PROG_MULTI 0x8 // Have Seen write bytes at program address and increment -#define STATE_PROTO_GET_CRC 0x10 // Have Seen compute & return a CRC -#define STATE_PROTO_GET_OTP 0x20 // Have Seen read a byte from OTP at the given address -#define STATE_PROTO_GET_SN 0x40 // Have Seen read a word from UDID area ( Serial) at the given address -#define STATE_PROTO_GET_CHIP 0x80 // Have Seen read chip version (MCU IDCODE) -#define STATE_PROTO_GET_CHIP_DES 0x100 // Have Seen read chip version In ASCII -#define STATE_PROTO_BOOT 0x200 // Have Seen boot the application +#define STATE_PROTO_GET_SYNC 0x1 // Have Seen NOP for re-establishing sync +#define STATE_PROTO_GET_DEVICE 0x2 // Have Seen get device ID bytes +#define STATE_PROTO_CHIP_ERASE 0x4 // Have Seen erase program area and reset program address +#define STATE_PROTO_PROG_MULTI 0x8 // Have Seen write bytes at program address and increment +#define STATE_PROTO_GET_CRC 0x10 // Have Seen compute & return a CRC +#define STATE_PROTO_GET_OTP 0x20 // Have Seen read a byte from OTP at the given address +#define STATE_PROTO_GET_SN 0x40 // Have Seen read a word from UDID area ( Serial) at the given address +#define STATE_PROTO_GET_CHIP 0x80 // Have Seen read chip version (MCU IDCODE) +#define STATE_PROTO_GET_CHIP_DES 0x100 // Have Seen read chip version In ASCII +#define STATE_PROTO_BOOT 0x200 // Have Seen boot the application #if defined(TARGET_HW_PX4_PIO_V1) #define STATE_ALLOWS_ERASE (STATE_PROTO_GET_SYNC) @@ -246,7 +253,7 @@ inline void cout(uint8_t *buf, unsigned len) #define arch_flash_lock flash_lock #define arch_flash_unlock flash_unlock -#define arch_setvtor(address) SCB_VTOR = address; +#define arch_setvtor(address) SCB_VTOR = (uint32_t)address; #endif @@ -283,22 +290,11 @@ buf_get(void) return ret; } -static void -do_jump(uint32_t stacktop, uint32_t entrypoint) -{ - asm volatile( - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stacktop), "r"(entrypoint) :); - - // just to keep noreturn happy - for (;;) ; -} - void jump_to_app() { const uint32_t *app_base = (const uint32_t *)APP_LOAD_ADDRESS; + const uint32_t *vec_base = (const uint32_t *)app_base; /* * We refuse to program the first word of the app until the upload is marked @@ -308,6 +304,80 @@ jump_to_app() return; } +#ifdef BOOTLOADER_USE_TOC + +#ifdef BOOTLOADER_USE_SECURITY + crypto_init(); +#endif + + const image_toc_entry_t *toc_entries; + uint8_t len; + uint8_t i = 0; + + /* When secure btl is used, the address comes from the TOC */ + app_base = (const uint32_t *)0; + vec_base = (const uint32_t *)0; + + /* TOC not found or empty, stay in btl */ + if (!find_toc(&toc_entries, &len)) { + return; + } + + /* Verify the first entry, containing the TOC itself */ + if (!verify_app(0, toc_entries)) { + /* Image verification failed, stay in btl */ + return; + } + + /* TOC is verified, loop through all the apps and perform crypto ops */ + for (i = 0; i < len; i++) { + /* Verify app, if needed. i == 0 is already verified */ + if (i != 0 && + toc_entries[i].flags1 & TOC_FLAG1_CHECK_SIGNATURE && + !verify_app(i, toc_entries)) { + /* Signature check failed, don't process this app */ + continue; + } + + /* Check if this app needs decryption */ + if (toc_entries[i].flags1 & TOC_FLAG1_DECRYPT && + !decrypt_app(i, toc_entries)) { + /* Decryption / authenticated decryption failed, skip this app */ + continue; + } + + /* Check if this app needs to be copied to RAM */ + if (toc_entries[i].flags1 & TOC_FLAG1_COPY) { + /* TOC is verified, so we assume that the addresses are good */ + memcpy(toc_entries[i].target, toc_entries[i].start, + (uintptr_t)toc_entries[i].end - (uintptr_t)toc_entries[i].start); + } + + /* Check if this app is bootable, if so set the app_base */ + if (toc_entries[i].flags1 & TOC_FLAG1_BOOT) { + app_base = get_base_addr(&toc_entries[i]); + } + + /* Check if this app has vectors, if so set the vec_base */ + if (toc_entries[i].flags1 & TOC_FLAG1_VTORS) { + vec_base = get_base_addr(&toc_entries[i]); + } + } + + if (app_base == 0) { + /* No bootable app found in TOC, bail out */ + return; + } + + if (vec_base == 0) { + /* No separate vectors block, vectors come along with the app */ + vec_base = app_base; + } + +#else + + /* These checks are arm specific, and not needed when using TOC */ + /* * The second word of the app is the entrypoint; it must point within the * flash area (or we have a bad flash). @@ -320,6 +390,8 @@ jump_to_app() return; } +#endif + /* just for paranoia's sake */ arch_flash_lock(); @@ -336,10 +408,10 @@ jump_to_app() board_deinit(); /* switch exception handlers to the application */ - arch_setvtor(APP_LOAD_ADDRESS); + arch_setvtor(vec_base); - /* extract the stack and entrypoint from the app vector table and go */ - do_jump(app_base[0], app_base[1]); + /* make arch specific jump to app */ + arch_do_jump(app_base); } volatile unsigned timer[NTIMERS]; diff --git a/platforms/nuttx/src/bootloader/bl.h b/platforms/nuttx/src/bootloader/common/bl.h similarity index 92% rename from platforms/nuttx/src/bootloader/bl.h rename to platforms/nuttx/src/bootloader/common/bl.h index 7c63f7344332..48a11cf4d8eb 100644 --- a/platforms/nuttx/src/bootloader/bl.h +++ b/platforms/nuttx/src/bootloader/common/bl.h @@ -106,13 +106,14 @@ extern uint32_t board_get_devices(void); extern void clock_deinit(void); extern uint32_t flash_func_sector_size(unsigned sector); extern void flash_func_erase_sector(unsigned sector); -extern void flash_func_write_word(uint32_t address, uint32_t word); -extern uint32_t flash_func_read_word(uint32_t address); -extern uint32_t flash_func_read_otp(uint32_t address); -extern uint32_t flash_func_read_sn(uint32_t address); +extern void flash_func_write_word(uintptr_t address, uint32_t word); +extern uint32_t flash_func_read_word(uintptr_t address); +extern uint32_t flash_func_read_otp(uintptr_t address); +extern uint32_t flash_func_read_sn(uintptr_t address); extern void arch_flash_lock(void); extern void arch_flash_unlock(void); -extern void arch_setvtor(uint32_t address); +extern void arch_setvtor(const uint32_t *address); +extern void arch_do_jump(const uint32_t *app_base); void arch_systic_init(void); void arch_systic_deinit(void); diff --git a/platforms/nuttx/src/bootloader/cdcacm.h b/platforms/nuttx/src/bootloader/common/cdcacm.h similarity index 100% rename from platforms/nuttx/src/bootloader/cdcacm.h rename to platforms/nuttx/src/bootloader/common/cdcacm.h diff --git a/platforms/nuttx/src/bootloader/common/crypto.c b/platforms/nuttx/src/bootloader/common/crypto.c new file mode 100644 index 000000000000..12bf011dd985 --- /dev/null +++ b/platforms/nuttx/src/bootloader/common/crypto.c @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 "image_toc.h" +#include "hw_config.h" + +#ifdef BOOTLOADER_USE_SECURITY + +#include + +bool verify_app(uint16_t idx, const image_toc_entry_t *toc_entries) +{ + volatile uint8_t *app_signature_ptr = NULL; + volatile size_t len = 0; + bool ret; + + uint8_t sig_idx = toc_entries[idx].signature_idx; + uint8_t sig_key = toc_entries[idx].signature_key; + crypto_session_handle_t handle = crypto_open(BOOTLOADER_SIGNING_ALGORITHM); + app_signature_ptr = (volatile uint8_t *)toc_entries[sig_idx].start; + len = (size_t)toc_entries[idx].end - (size_t)toc_entries[idx].start; + + ret = crypto_signature_check(handle, sig_key, (const uint8_t *)app_signature_ptr, + (const uint8_t *)toc_entries[idx].start, len); + + crypto_close(&handle); + return ret; +} + +bool decrypt_app(uint16_t idx, const image_toc_entry_t *toc_entries) +{ + /* + * Not implemented yet. + */ + return false; +} + +#endif //BOOTLOADER_USE_SECURITY diff --git a/platforms/nuttx/src/bootloader/common/crypto.h b/platforms/nuttx/src/bootloader/common/crypto.h new file mode 100644 index 000000000000..daf5c4df6693 --- /dev/null +++ b/platforms/nuttx/src/bootloader/common/crypto.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 crypto.h + * + * Wrapper for the crypto stuff + * + */ + +#pragma once + +/* Using security always needs TOC (but TOC could be used without security) */ +#if defined(BOOTLOADER_USE_SECURITY) +# define BOOTLOADER_USE_TOC + +#include + +#include "hw_config.h" +#include "image_toc.h" + +bool verify_app(uint16_t idx, const image_toc_entry_t *toc_entries); + +bool decrypt_app(uint16_t idx, const image_toc_entry_t *toc_entries); + +#else + +# if defined(BOOTLOADER_USE_TOC) + +/* No security, application verification passes always */ + +static inline bool verify_app(uint16_t idx, const image_toc_entry_t *toc_entries) {return true;} + +/* No security, decrypting is not possible */ + +static inline bool decrypt_app(uint16_t idx, const image_toc_entry_t *toc_entries) {return false;} + +# endif + +#endif // BOOTLOADER_USE_SECURITY diff --git a/platforms/nuttx/src/bootloader/common/image_toc.c b/platforms/nuttx/src/bootloader/common/image_toc.c new file mode 100644 index 000000000000..cdc6ca9db375 --- /dev/null +++ b/platforms/nuttx/src/bootloader/common/image_toc.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2020 Technology Innovation Institute. 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 "hw_config.h" + +#include +#include +#include +#include + +#include "image_toc.h" + +#include "bl.h" + +/* Helper macros to define flash start and end addresses, based on info from + * hw_config.h + */ +#define FLASH_START_ADDRESS (APP_LOAD_ADDRESS & (~(BOARD_FLASH_SIZE - 1))) +#define FLASH_END_ADDRESS (FLASH_START_ADDRESS + BOARD_FLASH_SIZE) + +bool find_toc(const image_toc_entry_t **toc_entries, uint8_t *len) +{ + const uintptr_t toc_start_u32 = APP_LOAD_ADDRESS + BOOT_DELAY_ADDRESS + 8; + const image_toc_start_t *toc_start = (const image_toc_start_t *)toc_start_u32; + const image_toc_entry_t *entry = (const image_toc_entry_t *)(toc_start_u32 + sizeof(image_toc_start_t)); + + int i = 0; + uint8_t sig_idx; + const uint32_t toc_end_magic = TOC_END_MAGIC; + + if (toc_start->magic == TOC_START_MAGIC && + toc_start->version <= TOC_VERSION) { + + /* Count the entries in TOC */ + while (i < MAX_TOC_ENTRIES && + (uintptr_t)&entry[i] <= FLASH_END_ADDRESS - sizeof(uintptr_t) && + memcmp(&entry[i], &toc_end_magic, sizeof(toc_end_magic))) { + i++; + } + + /* The number of toc entries found must be within bounds, and the + * application has to lie within the flashable area. Also ensure that + * end > start. + */ + + if (i <= MAX_TOC_ENTRIES && i > 0 && + (uintptr_t)entry[0].start == APP_LOAD_ADDRESS && + (uintptr_t)entry[0].end <= (FLASH_END_ADDRESS - sizeof(uintptr_t)) && + (uintptr_t)entry[0].end > (uintptr_t)entry[0].start) { + sig_idx = entry[0].signature_idx; + + /* The signature idx for the first app must be within the TOC, and + * the signature must be within the flash area, not overlapping the + * app. Also ensure that end > start. + */ + + if (sig_idx > 0 && + sig_idx < MAX_TOC_ENTRIES && + (uintptr_t)entry[sig_idx].start >= (uintptr_t)entry[0].end && + (uintptr_t)entry[sig_idx].end <= FLASH_END_ADDRESS && + (uintptr_t)entry[sig_idx].end > (uintptr_t)entry[sig_idx].start) { + *toc_entries = entry; + *len = i; + return true; + } + } + } + + *toc_entries = NULL; + *len = 0; + return false; +} diff --git a/platforms/nuttx/src/bootloader/common/lib/CMakeLists.txt b/platforms/nuttx/src/bootloader/common/lib/CMakeLists.txt new file mode 100644 index 000000000000..f79702114430 --- /dev/null +++ b/platforms/nuttx/src/bootloader/common/lib/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(bootloader_lib + cdcacm.c + uart.c + flash_cache.c +) + +add_dependencies(bootloader_lib prebuild_targets) diff --git a/platforms/nuttx/src/bootloader/lib/cdcacm.c b/platforms/nuttx/src/bootloader/common/lib/cdcacm.c similarity index 100% rename from platforms/nuttx/src/bootloader/lib/cdcacm.c rename to platforms/nuttx/src/bootloader/common/lib/cdcacm.c diff --git a/platforms/nuttx/src/bootloader/lib/flash_cache.c b/platforms/nuttx/src/bootloader/common/lib/flash_cache.c similarity index 91% rename from platforms/nuttx/src/bootloader/lib/flash_cache.c rename to platforms/nuttx/src/bootloader/common/lib/flash_cache.c index b84201d9619e..b6e06a79280b 100644 --- a/platforms/nuttx/src/bootloader/lib/flash_cache.c +++ b/platforms/nuttx/src/bootloader/common/lib/flash_cache.c @@ -38,6 +38,7 @@ #include +extern ssize_t arch_flash_write(uintptr_t address, const void *buffer, size_t buflen); flash_cache_line_t flash_cache[FC_NUMBER_LINES]; @@ -56,7 +57,7 @@ inline void fc_reset(void) flash_cache[0].start_address = APP_LOAD_ADDRESS; } -static inline flash_cache_line_t *fc_line_select(uint32_t address) +static inline flash_cache_line_t *fc_line_select(uintptr_t address) { for (unsigned w = 0; w < FC_NUMBER_LINES; w++) { if (flash_cache[w].start_address == (address & FC_ADDRESS_MASK)) { @@ -75,8 +76,8 @@ inline int fc_is_dirty(flash_cache_line_t *fl) int fc_flush(flash_cache_line_t *fl) { - size_t bytes = (fl->index + 1) * sizeof(fl->words[0]); - size_t rv = up_progmem_write(fl->start_address, fl->words, bytes); + const size_t bytes = sizeof(fl->words); + size_t rv = arch_flash_write(fl->start_address, fl->words, bytes); if (rv == bytes) { rv = 0; @@ -85,7 +86,7 @@ int fc_flush(flash_cache_line_t *fl) return rv; } -int fc_write(uint32_t address, uint32_t word) +int fc_write(uintptr_t address, uint32_t word) { flash_cache_line_t *fc = fc_line_select(address); flash_cache_line_t *fc1 = &flash_cache[1]; @@ -128,7 +129,7 @@ int fc_write(uint32_t address, uint32_t word) return rv; } -uint32_t fc_read(uint32_t address) +uint32_t fc_read(uintptr_t address) { // Assume a cache miss read from FLASH memory diff --git a/platforms/nuttx/src/bootloader/lib/flash_cache.h b/platforms/nuttx/src/bootloader/common/lib/flash_cache.h similarity index 95% rename from platforms/nuttx/src/bootloader/lib/flash_cache.h rename to platforms/nuttx/src/bootloader/common/lib/flash_cache.h index de180e8792b2..db5a1bd5ad88 100644 --- a/platforms/nuttx/src/bootloader/lib/flash_cache.h +++ b/platforms/nuttx/src/bootloader/common/lib/flash_cache.h @@ -58,12 +58,12 @@ // Cache line typedef struct flash_cache_line_t { uint32_t index; // Index of word written - uint32_t start_address; // cache tag (address in FLASH this is buffering) + uintptr_t start_address; // cache tag (address in FLASH this is buffering) uint32_t words[FC_NUMBER_WORDS]; // Buffered data } flash_cache_line_t; // Resets the cache - all lines flashed and cache_line[0] start_address == APP_LOAD_ADDRESS void fc_reset(void); // Cache operations -uint32_t fc_read(uint32_t address); -int fc_write(uint32_t address, uint32_t word); +uint32_t fc_read(uintptr_t address); +int fc_write(uintptr_t address, uint32_t word); diff --git a/platforms/nuttx/src/bootloader/lib/systick.h b/platforms/nuttx/src/bootloader/common/lib/systick.h similarity index 100% rename from platforms/nuttx/src/bootloader/lib/systick.h rename to platforms/nuttx/src/bootloader/common/lib/systick.h diff --git a/platforms/nuttx/src/bootloader/lib/uart.c b/platforms/nuttx/src/bootloader/common/lib/uart.c similarity index 100% rename from platforms/nuttx/src/bootloader/lib/uart.c rename to platforms/nuttx/src/bootloader/common/lib/uart.c diff --git a/platforms/nuttx/src/bootloader/uart.h b/platforms/nuttx/src/bootloader/common/uart.h similarity index 100% rename from platforms/nuttx/src/bootloader/uart.h rename to platforms/nuttx/src/bootloader/common/uart.h diff --git a/platforms/nuttx/src/bootloader/lib/CMakeLists.txt b/platforms/nuttx/src/bootloader/lib/CMakeLists.txt deleted file mode 100644 index ae021faa7eb0..000000000000 --- a/platforms/nuttx/src/bootloader/lib/CMakeLists.txt +++ /dev/null @@ -1,41 +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. -# -############################################################################ - -add_library(bootloader_lib - systick.c - cdcacm.c - uart.c - flash_cache.c -) - -add_dependencies(bootloader_lib prebuild_targets) diff --git a/platforms/nuttx/src/bootloader/stm/CMakeLists.txt b/platforms/nuttx/src/bootloader/stm/CMakeLists.txt new file mode 100644 index 000000000000..36843c5694ef --- /dev/null +++ b/platforms/nuttx/src/bootloader/stm/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/CMakeLists.txt b/platforms/nuttx/src/bootloader/stm/stm32_common/CMakeLists.txt new file mode 100644 index 000000000000..8395e2a1aeb5 --- /dev/null +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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(arch_bootloader + main.c + systick.c +) + +target_link_libraries(arch_bootloader + PRIVATE + bootloader_lib + nuttx_arch +) diff --git a/platforms/nuttx/src/bootloader/stm32h7/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c similarity index 95% rename from platforms/nuttx/src/bootloader/stm32h7/main.c rename to platforms/nuttx/src/bootloader/stm/stm32_common/main.c index 72221b984aba..894834f078fa 100644 --- a/platforms/nuttx/src/bootloader/stm32h7/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -424,22 +424,27 @@ clock_deinit(void) } -inline void arch_flash_lock(void) +void arch_flash_lock(void) { stm32h7_flash_lock(STM32_FLASH_BANK1); stm32h7_flash_lock(STM32_FLASH_BANK2); } -inline void arch_flash_unlock(void) +void arch_flash_unlock(void) { fc_reset(); stm32h7_flash_unlock(STM32_FLASH_BANK1); stm32h7_flash_unlock(STM32_FLASH_BANK2); } -inline void arch_setvtor(uint32_t address) +ssize_t arch_flash_write(uintptr_t address, const void *buffer, size_t buflen) { - putreg32(address, NVIC_VECTAB); + return up_progmem_write(address, buffer, buflen); +} + +inline void arch_setvtor(const uint32_t *address) +{ + putreg32((uint32_t)address, NVIC_VECTAB); } uint32_t @@ -470,13 +475,13 @@ flash_func_erase_sector(unsigned sector) } void -flash_func_write_word(uint32_t address, uint32_t word) +flash_func_write_word(uintptr_t address, uint32_t word) { address += APP_LOAD_ADDRESS; fc_write(address, word); } -uint32_t flash_func_read_word(uint32_t address) +uint32_t flash_func_read_word(uintptr_t address) { if (address & 3) { @@ -489,7 +494,7 @@ uint32_t flash_func_read_word(uint32_t address) uint32_t -flash_func_read_otp(uint32_t address) +flash_func_read_otp(uintptr_t address) { return 0; } @@ -545,7 +550,7 @@ int check_silicon(void) } uint32_t -flash_func_read_sn(uint32_t address) +flash_func_read_sn(uintptr_t address) { // read a byte out from unique chip ID area // it's 12 bytes, or 3 words. @@ -611,6 +616,23 @@ led_toggle(unsigned led) # define SCB_CPACR (*((volatile uint32_t *) (((0xE000E000UL) + 0x0D00UL) + 0x088))) #endif +/* Make the actual jump to app */ +void +arch_do_jump(const uint32_t *app_base) +{ + /* extract the stack and entrypoint from the app vector table and go */ + uint32_t stacktop = app_base[0]; + uint32_t entrypoint = app_base[1]; + + asm volatile( + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stacktop), "r"(entrypoint) :); + + // just to keep noreturn happy + for (;;) ; +} + int bootloader_main(void) { diff --git a/platforms/nuttx/src/bootloader/lib/systick.c b/platforms/nuttx/src/bootloader/stm/stm32_common/systick.c similarity index 99% rename from platforms/nuttx/src/bootloader/lib/systick.c rename to platforms/nuttx/src/bootloader/stm/stm32_common/systick.c index e75dfce485c6..3bc2d0072001 100644 --- a/platforms/nuttx/src/bootloader/lib/systick.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/systick.c @@ -32,7 +32,7 @@ ****************************************************************************/ #include "arm_arch.h" -#include "systick.h" +#include "lib/systick.h" #include uint8_t systick_get_countflag(void) diff --git a/platforms/nuttx/src/bootloader/stm/stm32h7/CMakeLists.txt b/platforms/nuttx/src/bootloader/stm/stm32h7/CMakeLists.txt new file mode 100644 index 000000000000..4e7642951b9a --- /dev/null +++ b/platforms/nuttx/src/bootloader/stm/stm32h7/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. 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_subdirectory(../stm32_common arch_bootloader) diff --git a/platforms/nuttx/src/canbootloader/CMakeLists.txt b/platforms/nuttx/src/canbootloader/CMakeLists.txt index be91358d055d..8ca45a1e871f 100644 --- a/platforms/nuttx/src/canbootloader/CMakeLists.txt +++ b/platforms/nuttx/src/canbootloader/CMakeLists.txt @@ -40,6 +40,7 @@ px4_add_library(canbootloader sched/timer.c uavcan/main.c util/random.c + util/cxx_init.c ) include_directories(include) diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt new file mode 100644 index 000000000000..eccd131c6dd6 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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(../../../include) + +px4_add_library(arch_canbootloader + board_identity.c + drivers/can/driver.c +) + +target_link_libraries(arch_canbootloader + PRIVATE + arch_watchdog_iwdg +) diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c new file mode 100644 index 000000000000..a716ad2f12df --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/board_identity.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file board_identity.c + * Implementation of STM32 based Board identity API + */ + +#include +#include +#include + +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +int board_get_mfguid(mfguid_t mfgid) +{ + uint32_t *chip_uuid = (uint32_t *) STM32_SYSMEM_UID; + uint32_t *rv = (uint32_t *) &mfgid[0]; + + for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + *rv++ = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + } + + return PX4_CPU_MFGUID_BYTE_LENGTH; +} diff --git a/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c new file mode 100644 index 000000000000..5a90ea708fde --- /dev/null +++ b/platforms/nuttx/src/canbootloader/arch/stm/stm32f7/drivers/can/driver.c @@ -0,0 +1,566 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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. + * + ****************************************************************************/ + +#include +#include "boot_config.h" + +#include +#include +#include +#include +#include + +#include "chip.h" +#include +#include "nvic.h" + +#include "board.h" +#include +#include "can.h" +#include "timer.h" + +#include + +#include + +#define INAK_TIMEOUT 65535 + +#define CAN_TX_TIMEOUT_MS (200 /(1000/(1000000/CONFIG_USEC_PER_TICK))) + +#define SJW_POS 24 +#define BS1_POS 16 +#define BS2_POS 20 + +#define CAN_TSR_RQCP_SHFTS 8 + +#define FILTER_ID 1 +#define FILTER_MASK 2 + +#if STM32_PCLK1_FREQUENCY == 54000000 +/* Sample 85.7 % */ +# define QUANTA 18 +# define BS1_VALUE 14 +# define BS2_VALUE 1 +#elif STM32_PCLK1_FREQUENCY == 48000000 +/* Sample 85.7 % */ +# define QUANTA 16 +# define BS1_VALUE 12 +# define BS2_VALUE 1 +#elif STM32_PCLK1_FREQUENCY == 45000000 || STM32_PCLK1_FREQUENCY == 36000000 +/* Sample 88.9 % */ +# define QUANTA 9 +# define BS1_VALUE 6 +# define BS2_VALUE 0 +#elif STM32_PCLK1_FREQUENCY == 42000000 +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#else +# warning Undefined QUANTA bsed on Clock Rate +/* Sample 85.7 % */ +# define QUANTA 14 +# define BS1_VALUE 10 +# define BS2_VALUE 1 +#endif + +#define CAN_1MBAUD_SJW 0 +#define CAN_1MBAUD_BS1 BS1_VALUE +#define CAN_1MBAUD_BS2 BS2_VALUE +#define CAN_1MBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/1000000/QUANTA) + +#define CAN_500KBAUD_SJW 0 +#define CAN_500KBAUD_BS1 BS1_VALUE +#define CAN_500KBAUD_BS2 BS2_VALUE +#define CAN_500KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/500000/QUANTA) + +#define CAN_250KBAUD_SJW 0 +#define CAN_250KBAUD_BS1 BS1_VALUE +#define CAN_250KBAUD_BS2 BS2_VALUE +#define CAN_250KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/250000/QUANTA) + +#define CAN_125KBAUD_SJW 0 +#define CAN_125KBAUD_BS1 BS1_VALUE +#define CAN_125KBAUD_BS2 BS2_VALUE +#define CAN_125KBAUD_PRESCALER (STM32_PCLK1_FREQUENCY/125000/QUANTA) + +#define CAN_BTR_LBK_SHIFT 30 + +// Number of CPU cycles for a single bit time at the supported speeds +#define CAN_125KBAUD_BIT_CYCLES (8*(TIMER_HRT_CYCLES_PER_US)) + +#define CAN_BAUD_TIME_IN_MS 200 +#define CAN_BAUD_SAMPLES_NEEDED 32 +#define CAN_BAUD_SAMPLES_DISCARDED 8 + +static inline uint32_t read_msr_rx(void) +{ + return getreg32(STM32_CAN1_MSR) & CAN_MSR_RX; +} + +static uint32_t read_msr(time_hrt_cycles_t *now) +{ + __asm__ __volatile__("\tcpsid i\n"); + *now = timer_hrt_read(); + uint32_t msr = read_msr_rx(); + __asm__ __volatile__("\tcpsie i\n"); + return msr; +} + +static int read_bits_times(time_hrt_cycles_t *times, size_t max) +{ + uint32_t samplecnt = 0; + bl_timer_id ab_timer = timer_allocate(modeTimeout | modeStarted, CAN_BAUD_TIME_IN_MS, 0); + time_ref_t ab_ref = timer_ref(ab_timer); + uint32_t msr; + uint32_t last_msr = read_msr(times); + + while (samplecnt < max && !timer_ref_expired(ab_ref)) { + do { + msr = read_msr(×[samplecnt]); + } while (!(msr ^ last_msr) && !timer_ref_expired(ab_ref)); + + last_msr = msr; + samplecnt++; + } + + timer_free(ab_timer); + return samplecnt; +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a can_speed_t to a bit rate in Hz + * + * Input Parameters: + * can_speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + * Returned value: + * Bit rate in Hz + * + ****************************************************************************/ +int can_speed2freq(can_speed_t speed) + +{ + return 1000000 >> (CAN_1MBAUD - speed); +} + +/**************************************************************************** + * Name: can_speed2freq + * + * Description: + * This function maps a frequency in Hz to a can_speed_t in the range + * CAN_125KBAUD to CAN_1MBAUD. + * + * Input Parameters: + * freq - Bit rate in Hz + * + * Returned value: + * A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * + ****************************************************************************/ +can_speed_t can_freq2speed(int freq) +{ + return (freq == 1000000u ? CAN_1MBAUD : freq == 500000u ? CAN_500KBAUD : freq == 250000u ? CAN_250KBAUD : CAN_125KBAUD); +} + +/**************************************************************************** + * Name: can_tx + * + * Description: + * This function is called to transmit a CAN frame using the supplied + * mailbox. It will busy wait on the mailbox if not available. + * + * Input Parameters: + * message_id - The CAN message's EXID field + * length - The number of bytes of data - the DLC field + * message - A pointer to 8 bytes of data to be sent (all 8 bytes will be + * loaded into the CAN transmitter but only length bytes will + * be sent. + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * The CAN_OK of the data sent or CAN_ERROR if a time out occurred + * + ****************************************************************************/ +uint8_t can_tx(uint32_t message_id, size_t length, const uint8_t *message, uint8_t mailbox) +{ + uint32_t data[2]; + + memcpy(data, message, sizeof(data)); + + /* + * Just block while waiting for the mailbox. + */ + + uint32_t mask = CAN_TSR_TME0 << mailbox; + + /* Reset the indication of timer expired */ + + timer_hrt_clear_wrap(); + uint32_t cnt = CAN_TX_TIMEOUT_MS; + + while ((getreg32(STM32_CAN1_TSR) & mask) == 0) { + if (timer_hrt_wrap()) { + timer_hrt_clear_wrap(); + + if (--cnt == 0) { + return CAN_ERROR; + } + } + } + + /* + * To allow detection of completion - Set the LEC to + * 'No error' state off all 1s + */ + + putreg32(CAN_ESR_LEC_MASK, STM32_CAN1_ESR); + + putreg32(length & CAN_TDTR_DLC_MASK, STM32_CAN1_TDTR(mailbox)); + putreg32(data[0], STM32_CAN1_TDLR(mailbox)); + putreg32(data[1], STM32_CAN1_TDHR(mailbox)); + putreg32((message_id << CAN_TIR_EXID_SHIFT) | CAN_TIR_IDE | CAN_TIR_TXRQ, + STM32_CAN1_TIR(mailbox)); + return CAN_OK; +} + +/**************************************************************************** + * Name: can_rx + * + * Description: + * This function is called to receive a CAN frame from a supplied fifo. + * It does not block if there is not available, but returns 0 + * + * Input Parameters: + * message_id - A pointer to return the CAN message's EXID field + * length - A pointer to return the number of bytes of data - the DLC field + * message - A pointer to return 8 bytes of data to be sent (all 8 bytes will + * be written from the CAN receiver but only length bytes will be sent. + * fifo A can_fifo_mailbox_t fifixxx value to choose the incoming fifo. + * + * Returned value: + * The length of the data read or 0 if the fifo was empty + * + ****************************************************************************/ +uint8_t can_rx(uint32_t *message_id, size_t *length, uint8_t *message, uint8_t fifo) +{ + uint32_t data[2]; + uint8_t rv = 0; + const uint32_t fifos[] = { STM32_CAN1_RF0R, STM32_CAN1_RF1R }; + + if (getreg32(fifos[fifo & 1]) & CAN_RFR_FMP_MASK) { + + rv = 1; + /* If so, process it */ + + *message_id = (getreg32(STM32_CAN1_RIR(fifo)) & CAN_RIR_EXID_MASK) >> + CAN_RIR_EXID_SHIFT; + *length = (getreg32(STM32_CAN1_RDTR(fifo)) & CAN_RDTR_DLC_MASK) >> + CAN_RDTR_DLC_SHIFT; + data[0] = getreg32(STM32_CAN1_RDLR(fifo)); + data[1] = getreg32(STM32_CAN1_RDHR(fifo)); + + putreg32(CAN_RFR_RFOM, fifos[fifo & 1]); + + memcpy(message, data, sizeof(data)); + } + + return rv; +} + +/**************************************************************************** + * Name: can_autobaud + * + * Description: + * This function will attempt to detect the bit rate in use on the CAN + * interface until the timeout provided expires or the successful detection + * occurs. + * + * It will initialize the CAN block for a given bit rate + * to test that a message can be received. The CAN interface is left + * operating at the detected bit rate and in CAN_Mode_Normal mode. + * + * Input Parameters: + * can_speed - A pointer to return detected can_speed_t from CAN_UNKNOWN to + * CAN_1MBAUD + * timeout - The timer id of a timer to use as the maximum time to wait for + * successful bit rate detection. This timer may be not running + * in which case the auto baud code will try indefinitely to + * detect the bit rate. + * + * Returned value: + * CAN_OK - on Success or a CAN_BOOT_TIMEOUT + * + ****************************************************************************/ +int can_autobaud(can_speed_t *can_speed, bl_timer_id timeout) +{ + *can_speed = CAN_UNKNOWN; + + volatile int attempt = 0; + /* Threshold are at 1.5 Bit times */ + + /* + * We are here because there was a reset or the app invoked + * the bootloader with no bit rate set. + */ + + time_hrt_cycles_t bit_time; + time_hrt_cycles_t min_cycles; + int sample; + can_speed_t speed = CAN_125KBAUD; + + time_hrt_cycles_t samples[128]; + + while (1) { + + + while (1) { + + min_cycles = ULONG_MAX; + int samplecnt = read_bits_times(samples, arraySize(samples)); + + if (timer_expired(timeout)) { + return CAN_BOOT_TIMEOUT; + } + + if ((getreg32(STM32_CAN1_RF0R) | getreg32(STM32_CAN1_RF1R)) & + CAN_RFR_FMP_MASK) { + *can_speed = speed; + can_init(speed, CAN_Mode_Normal); + return CAN_OK; + } + + if (samplecnt < CAN_BAUD_SAMPLES_NEEDED) { + continue; + } + + for (sample = 0; sample < samplecnt; sample += 2) { + bit_time = samples[sample] = timer_hrt_elapsed(samples[sample], samples[sample + 1]); + + if (sample > CAN_BAUD_SAMPLES_DISCARDED && bit_time < min_cycles) { + min_cycles = bit_time; + } + } + + break; + } + + uint32_t bit34 = CAN_125KBAUD_BIT_CYCLES - CAN_125KBAUD_BIT_CYCLES / 4; + samples[1] = min_cycles; + speed = CAN_125KBAUD; + + while (min_cycles < bit34 && speed < CAN_1MBAUD) { + speed++; + bit34 /= 2; + } + + attempt++; + can_init(speed, CAN_Mode_Silent); + + + } /* while(1) */ + + return CAN_OK; +} + +/**************************************************************************** + * Name: can_init + * + * Description: + * This function is used to initialize the CAN block for a given bit rate and + * mode. + * + * Input Parameters: + * speed - A can_speed_t from CAN_125KBAUD to CAN_1MBAUD + * mode - One of the can_mode_t of Normal, LoopBack and Silent or + * combination thereof. + * + * Returned value: + * OK - on Success or a negate errno value + * + ****************************************************************************/ +int can_init(can_speed_t speed, can_mode_t mode) +{ + int speedndx = speed - 1; + /* + * TODO: use full-word writes to reduce the number of loads/stores. + * + * Also consider filter use -- maybe set filters for all the message types we + * want. */ + const uint32_t bitrates[] = { + (CAN_125KBAUD_SJW << SJW_POS) | + (CAN_125KBAUD_BS1 << BS1_POS) | + (CAN_125KBAUD_BS2 << BS2_POS) | (CAN_125KBAUD_PRESCALER - 1), + + (CAN_250KBAUD_SJW << SJW_POS) | + (CAN_250KBAUD_BS1 << BS1_POS) | + (CAN_250KBAUD_BS2 << BS2_POS) | (CAN_250KBAUD_PRESCALER - 1), + + (CAN_500KBAUD_SJW << SJW_POS) | + (CAN_500KBAUD_BS1 << BS1_POS) | + (CAN_500KBAUD_BS2 << BS2_POS) | (CAN_500KBAUD_PRESCALER - 1), + + (CAN_1MBAUD_SJW << SJW_POS) | + (CAN_1MBAUD_BS1 << BS1_POS) | + (CAN_1MBAUD_BS2 << BS2_POS) | (CAN_1MBAUD_PRESCALER - 1) + }; + + /* Remove Unknow Offset */ + if (speedndx < 0 || speedndx > (int)arraySize(bitrates)) { + return -EINVAL; + } + + uint32_t timeout; + /* + * Reset state is 0x0001 0002 CAN_MCR_DBF|CAN_MCR_SLEEP + * knock down Sleep and raise CAN_MCR_INRQ + */ + + putreg32(CAN_MCR_DBF | CAN_MCR_INRQ, STM32_CAN1_MCR); + + /* Wait until initialization mode is acknowledged */ + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) != 0) { + /* We are in initialization mode */ + + break; + } + } + + if (timeout < 1) { + /* + * Initialization failed, not much we can do now other than try a normal + * startup. */ + return -ETIME; + } + + + putreg32(bitrates[speedndx] | mode << CAN_BTR_LBK_SHIFT, STM32_CAN1_BTR); + putreg32(CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_DBF | CAN_MCR_TXFP, STM32_CAN1_MCR); + + for (timeout = INAK_TIMEOUT; timeout > 0; timeout--) { + if ((getreg32(STM32_CAN1_MSR) & CAN_MSR_INAK) == 0) { + /* We are in initialization mode */ + + break; + } + } + + if (timeout < 1) { + return -ETIME; + } + + /* + * CAN filter initialization -- accept everything on RX FIFO 0, and only + * GetNodeInfo requests on RX FIFO 1. */ + + /* Set FINIT - Initialization mode for the filters- */ + putreg32(CAN_FMR_FINIT, STM32_CAN1_FMR); + + putreg32(0, STM32_CAN1_FA1R); /* Disable all filters */ + + putreg32(3, STM32_CAN1_FS1R); /* Single 32-bit scale configuration for filters 0 and 1 */ + + /* Filter 0 masks -- DTIDGetNodeInfo requests only */ + + uavcan_protocol_t protocol; + + protocol.id.u32 = 0; + protocol.ser.type_id = DTIDReqGetNodeInfo; + protocol.ser.service_not_message = true; + protocol.ser.request_not_response = true; + + /* Set the Filter ID */ + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_ID)); + + /* Set the Filter Mask */ + protocol.ser.type_id = BIT_MASK(LengthUavCanServiceTypeID); + + putreg32(protocol.id.u32 << CAN_RIR_EXID_SHIFT, STM32_CAN1_FIR(0, FILTER_MASK)); + + /* Filter 1 masks -- everything is don't-care */ + putreg32(0, STM32_CAN1_FIR(1, FILTER_ID)); + putreg32(0, STM32_CAN1_FIR(1, FILTER_MASK)); + + putreg32(0, STM32_CAN1_FM1R); /* Mask mode for all filters */ + putreg32(1, STM32_CAN1_FFA1R); /* FIFO 1 for filter 0, FIFO 0 for the + * rest of filters */ + putreg32(3, STM32_CAN1_FA1R); /* Enable filters 0 and 1 */ + + /* Clear FINIT - Active mode for the filters- */ + + putreg32(0, STM32_CAN1_FMR); + + return OK; +} + +/**************************************************************************** + * Name: can_cancel_on_error + * + * Description: + * This function will test for transition completion or any error. + * If the is a error the the transmit will be aborted. + * + * Input Parameters: + * mailbox - A can_fifo_mailbox_t MBxxx value to choose the outgoing + * mailbox. + * + * Returned value: + * CAN_OK - on Success or a CAN_ERROR if the cancellation was needed + * + ****************************************************************************/ +void can_cancel_on_error(uint8_t mailbox) +{ + uint32_t rvalue; + + /* Wait for completion the all 1's wat set in the tx code*/ + while (CAN_ESR_LEC_MASK == ((rvalue = (getreg32(STM32_CAN1_ESR) & CAN_ESR_LEC_MASK)))); + + /* Any errors */ + if (rvalue) { + + putreg32(0, STM32_CAN1_ESR); + + /* Abort the the TX in case of collision wuth NART false */ + + putreg32(CAN_TSR_ABRQ0 << (mailbox * CAN_TSR_RQCP_SHFTS), STM32_CAN1_TSR); + } +} diff --git a/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c b/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c index 897bca8718d9..a1ef574fce54 100644 --- a/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c +++ b/platforms/nuttx/src/canbootloader/common/nuttx_stubs.c @@ -40,6 +40,7 @@ #include "nuttx/arch.h" #include "arm_internal.h" +#include "boot_config.h" /**************************************************************************** * Pre-processor Definitions @@ -81,6 +82,12 @@ int main(int argc, char **argv); * ****************************************************************************/ +int __wrap_nxsem_wait_uninterruptible(void *sem) +{ + return 0; + +} + int __wrap_nxsem_wait(void *sem) { return 0; @@ -174,13 +181,27 @@ void __wrap_nx_start(void) * Name: malloc * * Description: - * This function hijacks the OS's malloc and provides no allocation + * This function hijacks the OS's malloc and provides no or small + * allocation * ****************************************************************************/ FAR void *malloc(size_t size) { +#if defined(OPT_SIMPLE_MALLOC_HEAP_SIZE) + static int aloc = 0; + static uint8_t mem[OPT_SIMPLE_MALLOC_HEAP_SIZE]; + void *rv = &mem[aloc]; + aloc += size; + + if (aloc > OPT_SIMPLE_MALLOC_HEAP_SIZE) { + PANIC(); + } + + return rv; +#else return NULL; +#endif } /**************************************************************************** diff --git a/platforms/nuttx/src/canbootloader/include/board.h b/platforms/nuttx/src/canbootloader/include/board.h index 36365b7082ac..6d25ffd796b6 100644 --- a/platforms/nuttx/src/canbootloader/include/board.h +++ b/platforms/nuttx/src/canbootloader/include/board.h @@ -57,6 +57,7 @@ typedef enum { fw_update_timeout, fw_update_invalid_crc, jump_to_app, + hardware_failure, } uiindication_t; #ifndef __ASSEMBLY__ diff --git a/platforms/nuttx/src/canbootloader/include/cxx_init.h b/platforms/nuttx/src/canbootloader/include/cxx_init.h new file mode 100644 index 000000000000..45feec841798 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/include/cxx_init.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * Name: cxx_initialize + * + * Description: + * This function initialises static C++N objects + * + * + * Input Parameters: + * None + * + * Returned value: + * None + * + ****************************************************************************/ +void cxx_initialize(void); diff --git a/platforms/nuttx/src/canbootloader/uavcan/main.c b/platforms/nuttx/src/canbootloader/uavcan/main.c index 225390f612af..d634715f4aa1 100644 --- a/platforms/nuttx/src/canbootloader/uavcan/main.c +++ b/platforms/nuttx/src/canbootloader/uavcan/main.c @@ -936,6 +936,9 @@ static void application_run(size_t fw_image_size, bootloader_app_shared_t *commo /* kill the systick interrupt */ putreg32(0, NVIC_SYSTICK_CTRL); + __asm volatile("dsb"); + __asm volatile("isb"); + putreg32(NVIC_INTCTRL_PENDSTCLR, NVIC_INTCTRL); /* and set a specific LED pattern */ board_indicate(jump_to_app); diff --git a/platforms/nuttx/src/canbootloader/util/cxx_init.c b/platforms/nuttx/src/canbootloader/util/cxx_init.c new file mode 100644 index 000000000000..9e7ea23a27f7 --- /dev/null +++ b/platforms/nuttx/src/canbootloader/util/cxx_init.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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 + +#ifdef CONFIG_HAVE_CXXINITIALIZE +#include "cxx_init.h" + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This type defines one entry in initialization array */ + +typedef CODE void (*initializer_t)(void); + +/**************************************************************************** + * External References + ****************************************************************************/ + +/* _sinit and _einit are symbols exported by the linker script that mark the + * beginning and the end of the C++ initialization section. + */ + +extern initializer_t _sinit; +extern initializer_t _einit; + +/* _stext and _etext are symbols exported by the linker script that mark the + * beginning and the end of text. + */ + +extern uintptr_t _stext; +extern uintptr_t _etext; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cxx_initialize + * + * Description: + * If C++ and C++ static constructors are supported, then this function + * must be provided by board-specific logic in order to perform + * initialization of the static C++ class instances. + * + * This function should then be called in the application-specific + * user_start logic in order to perform the C++ initialization. NOTE + * that no component of the core NuttX RTOS logic is involved; this + * function definition only provides the 'contract' between application + * specific C++ code and platform-specific toolchain support. + * + ****************************************************************************/ + +void cxx_initialize(void) +{ + static int inited = 0; + + if (inited == 0) { + initializer_t *initp; + + sinfo("_sinit: %p _einit: %p _stext: %p _etext: %p\n", + &_sinit, &_einit, &_stext, &_etext); + + /* Visit each entry in the initialization table */ + + for (initp = &_sinit; initp != &_einit; initp++) { + initializer_t initializer = *initp; + sinfo("initp: %p initializer: %p\n", initp, initializer); + + /* Make sure that the address is non-NULL and lies in the text + * region defined by the linker script. Some toolchains may put + * NULL values or counts in the initialization table. + */ + + if ((FAR void *)initializer >= (FAR void *)&_stext && + (FAR void *)initializer < (FAR void *)&_etext) { + sinfo("Calling %p\n", initializer); + initializer(); + } + } + + inited = 1; + } +} + +#endif diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 6a87f19a10d8..fbccd77a0f1f 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -33,11 +33,12 @@ # skip for px4_layer support on an IO board if(NOT PX4_BOARD MATCHES "io-v2") - - add_library(px4_layer + # Kernel side & nuttx flat build common sources + set(KERNEL_SRCS board_crashdump.c board_dma_alloc.c board_fat_dma_alloc.c + cdc_acm_check.cpp console_buffer.cpp cpuload.cpp gpio.c @@ -48,21 +49,38 @@ if(NOT PX4_BOARD MATCHES "io-v2") px4_manifest.cpp px4_mtd.cpp px4_24xxxx_mtd.c + px4_crypto.cpp + ) + + # Kernel side & nuttx flat build common libraries + set(KERNEL_LIBS + arch_board_reset + arch_board_critmon + arch_version + nuttx_sched ) - target_link_libraries(px4_layer - PRIVATE - arch_board_reset - arch_board_critmon - arch_version - nuttx_apps - nuttx_sched - px4_work_queue - uORB - ) + +if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") + # Build the NuttX user and kernel space px4 layers + include(${CMAKE_CURRENT_SOURCE_DIR}/px4_protected_layers.cmake) + else() + # Build the flat build px4_layer + include(${CMAKE_CURRENT_SOURCE_DIR}/px4_layer.cmake) +endif() + +else() + # Build the px4 layer for io_v2 add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c) endif() + add_dependencies(px4_layer prebuild_targets) add_subdirectory(gpio) add_subdirectory(srgbled) + +# Build px4_random +if (DEFINED PX4_CRYPTO) + add_library(px4_random nuttx_random.c) + target_link_libraries(px4_random PRIVATE nuttx_crypto) +endif() diff --git a/platforms/nuttx/src/px4/common/board_crashdump.c b/platforms/nuttx/src/px4/common/board_crashdump.c index 7758965ca177..035bc6044318 100644 --- a/platforms/nuttx/src/px4/common/board_crashdump.c +++ b/platforms/nuttx/src/px4/common/board_crashdump.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2017-2019, 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 @@ -54,6 +54,7 @@ #include "arm_internal.h" #include #include "nvic.h" +#include #if defined(CONFIG_STM32F7_BBSRAM) && defined(CONFIG_STM32F7_SAVE_CRASHDUMP) # define HAS_BBSRAM CONFIG_STM32F7_BBSRAM @@ -325,7 +326,7 @@ __EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const char pdump->info.stacks.user.size = CONFIG_IDLETHREAD_STACKSIZE; } else { - pdump->info.stacks.user.top = (uint32_t) rtcb->adj_stack_ptr; + pdump->info.stacks.user.top = (uint32_t) rtcb->stack_base_ptr + rtcb->adj_stack_size; pdump->info.stacks.user.size = (uint32_t) rtcb->adj_stack_size; } @@ -333,7 +334,7 @@ __EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const char /* Get the limits on the interrupt stack memory */ - pdump->info.stacks.interrupt.top = (uint32_t)&g_intstackbase; + pdump->info.stacks.interrupt.top = (uint32_t)&g_intstacktop; pdump->info.stacks.interrupt.size = (CONFIG_ARCH_INTERRUPTSTACK & ~3); /* If In interrupt Context save the interrupt stack data centered diff --git a/platforms/nuttx/src/px4/common/board_ctrl.c b/platforms/nuttx/src/px4/common/board_ctrl.c new file mode 100644 index 000000000000..15b4530f20b4 --- /dev/null +++ b/platforms/nuttx/src/px4/common/board_ctrl.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. 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_ctrl.c + * + * Provide a kernel-userspace boardctl_ioctl interface + */ + +#include +#include +#include +#include "board_config.h" + +#include +#include + +FAR const struct builtin_s g_kernel_builtins[] = { +#include +}; + +const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s); + +static struct { + ioctl_ptr_t ioctl_func; +} ioctl_ptrs[MAX_IOCTL_PTRS]; + +/************************************************************************************ + * Name: px4_register_boardct_ioctl + * + * Description: + * an interface function for kernel services to register an ioct handler for user side + * + ************************************************************************************/ + + +int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func) +{ + unsigned i = IOCTL_BASE_TO_IDX(base); + int ret = PX4_ERROR; + + if (i < MAX_IOCTL_PTRS) { + ioctl_ptrs[i].ioctl_func = func; + ret = PX4_OK; + } + + return ret; +} + +/************************************************************************************ + * Name: board_ioctl + * + * Description: + * implements board_ioctl for userspace-kernel interface + * + ************************************************************************************/ + +int board_ioctl(unsigned int cmd, uintptr_t arg) +{ + unsigned i = IOCTL_BASE_TO_IDX(cmd); + int ret = -EINVAL; + + if (i < MAX_IOCTL_PTRS && ioctl_ptrs[i].ioctl_func) { + ret = ioctl_ptrs[i].ioctl_func(cmd, arg); + } + + return ret; +} + +/************************************************************************************ + * Name: launch_kernel_builtin + * + * Description: + * launches a kernel-side build-in module + * + ************************************************************************************/ + +static int launch_kernel_builtin(int argc, char **argv) +{ + int i; + FAR const struct builtin_s *builtin = NULL; + + for (i = 0; i < g_n_kernel_builtins; i++) { + if (!strcmp(g_kernel_builtins[i].name, argv[0])) { + builtin = &g_kernel_builtins[i]; + break; + } + } + + if (builtin) { + /* This is running in the userspace thread, created by nsh, and + called via boardctl. Call the main directly */ + return builtin->main(argc, argv); + } + + return ENOENT; +} + +/************************************************************************************ + * Name: platform_ioctl + * + * Description: + * handle all miscellaneous platform level ioctls + * + ************************************************************************************/ + +static int platform_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = PX4_OK; + + switch (cmd) { + case PLATFORMIOCLAUNCH: { + platformioclaunch_t *data = (platformioclaunch_t *)arg; + data->ret = launch_kernel_builtin(data->argc, data->argv); + } + break; + + case PLATFORMIOCVBUSSTATE: { + platformiocvbusstate_t *data = (platformiocvbusstate_t *)arg; + data->ret = px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; + } + break; + + case PLATFORMIOCINDICATELOCKOUT: { + platformioclockoutstate_t *data = (platformioclockoutstate_t *)arg; + px4_arch_configgpio(data->enabled ? GPIO_nARMED : GPIO_nARMED_INIT); + } + break; + + case PLATFORMIOCGETLOCKOUT: { + platformioclockoutstate_t *data = (platformioclockoutstate_t *)arg; + data->enabled = px4_arch_gpioread(GPIO_nARMED); + } + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + + +/************************************************************************************ + * Name: kernel_ioctl_initialize + * + * Description: + * initializes the kernel-side ioctl interface + * + ************************************************************************************/ + +void kernel_ioctl_initialize(void) +{ + for (int i = 0; i < MAX_IOCTL_PTRS; i++) { + ioctl_ptrs[i].ioctl_func = NULL; + } + + px4_register_boardct_ioctl(_PLATFORMIOCBASE, platform_ioctl); +} diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp new file mode 100644 index 000000000000..5d9702152fbd --- /dev/null +++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp @@ -0,0 +1,372 @@ +/**************************************************************************** + * + * Copyright (c) 2019-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. + * + ****************************************************************************/ + +#if defined(CONFIG_SYSTEM_CDCACM) +__BEGIN_DECLS +#include +#include +#include +#include + +#include +#include + +extern int sercon_main(int c, char **argv); +extern int serdis_main(int c, char **argv); +__END_DECLS + +#include + +#include +#include + +#define USB_DEVICE_PATH "/dev/ttyACM0" + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) +# undef SERIAL_PASSTHRU_UBLOX_DEV +# if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5 +# endif +# if !defined(SERIAL_PASSTHRU_UBLOX_DEV) +# error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined" +# endif +#endif + +static struct work_s usb_serial_work; +static bool vbus_present_prev = false; +static int ttyacm_fd = -1; + +enum class UsbAutoStartState { + disconnected, + connecting, + connected, + disconnecting, +} usb_auto_start_state{UsbAutoStartState::disconnected}; + + +static void mavlink_usb_check(void *arg) +{ + int rescheduled = -1; + + uORB::SubscriptionData actuator_armed_sub{ORB_ID(actuator_armed)}; + + const bool armed = actuator_armed_sub.get().armed; + bool vbus_present = (board_read_VBUS_state() == PX4_OK); + bool locked_out = false; + + // If the hardware support RESET lockout that has nArmed ANDed with VBUS + // vbus_sense may drop during a param save which uses + // BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets + // while writing the params. If we are not armed and nARMRED is low + // we are in such a lock out so ignore changes on VBUS_SENSE during this + // time. +#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE) + locked_out = BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0; + + if (locked_out) { + vbus_present = vbus_present_prev; + } + +#endif + + + if (!armed && !locked_out) { + switch (usb_auto_start_state) { + case UsbAutoStartState::disconnected: + if (vbus_present && vbus_present_prev) { + if (sercon_main(0, nullptr) == EXIT_SUCCESS) { + usb_auto_start_state = UsbAutoStartState::connecting; + rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000)); + } + + } else if (vbus_present && !vbus_present_prev) { + // check again sooner if USB just connected + rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000)); + } + + break; + + case UsbAutoStartState::connecting: + if (vbus_present && vbus_present_prev) { + if (ttyacm_fd < 0) { + ttyacm_fd = ::open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + } + + if (ttyacm_fd >= 0) { + int bytes_available = 0; + int retval = ::ioctl(ttyacm_fd, FIONREAD, &bytes_available); + + if ((retval == OK) && (bytes_available >= 3)) { + char buffer[80]; + + // non-blocking read + int nread = ::read(ttyacm_fd, buffer, sizeof(buffer)); + +#if defined(DEBUG_BUILD) + + if (nread > 0) { + fprintf(stderr, "%d bytes\n", nread); + + for (int i = 0; i < nread; i++) { + fprintf(stderr, "|%X", buffer[i]); + } + + fprintf(stderr, "\n"); + } + +#endif // DEBUG_BUILD + + + if (nread > 0) { + // Mavlink reboot/shutdown command + // COMMAND_LONG (#76) with command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246) + static constexpr int MAVLINK_COMMAND_LONG_MIN_LENGTH = 41; + + if (nread >= MAVLINK_COMMAND_LONG_MIN_LENGTH) { + // scan buffer for mavlink COMMAND_LONG + for (int i = 0; i < nread - MAVLINK_COMMAND_LONG_MIN_LENGTH; i++) { + if ((buffer[i] == 0xFE) // Mavlink v1 start byte + && (buffer[i + 5] == 76) // 76=0x4C COMMAND_LONG + && (buffer[i + 34] == 246) // 246=0xF6 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN + ) { + // mavlink v1 COMMAND_LONG + // buffer[0]: start byte (0xFE for mavlink v1) + // buffer[3]: SYSID + // buffer[4]: COMPID + // buffer[5]: message id (COMMAND_LONG 76=0x4C) + // buffer[6-10]: COMMAND_LONG param 1 (little endian float) + // buffer[34]: COMMAND_LONG command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246/0xF6) + float param1_raw = 0; + memcpy(¶m1_raw, &buffer[i + 6], 4); + int param1 = roundf(param1_raw); + + syslog(LOG_INFO, "%s: Mavlink MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN param 1: %d (SYSID:%d COMPID:%d)\n", + USB_DEVICE_PATH, param1, buffer[i + 3], buffer[i + 4]); + + if (param1 == 1) { + // 1: Reboot autopilot + px4_reboot_request(false, 0); + + } else if (param1 == 2) { + // 2: Shutdown autopilot +#if defined(BOARD_HAS_POWER_CONTROL) + px4_shutdown_request(0); +#endif // BOARD_HAS_POWER_CONTROL + + } else if (param1 == 3) { + // 3: Reboot autopilot and keep it in the bootloader until upgraded. + px4_reboot_request(true, 0); + } + } + } + } + + + bool launch_mavlink = false; + bool launch_nshterm = false; + bool launch_passthru = false; + struct termios uart_config; + static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9; + + if (nread >= MAVLINK_HEARTBEAT_MIN_LENGTH) { + // scan buffer for mavlink HEARTBEAT (v1 & v2) + for (int i = 0; i < nread - MAVLINK_HEARTBEAT_MIN_LENGTH; i++) { + if ((buffer[i] == 0xFE) && (buffer[i + 1] == 9) && (buffer[i + 5] == 0)) { + // mavlink v1 HEARTBEAT + // buffer[0]: start byte (0xFE for mavlink v1) + // buffer[1]: length (9 for HEARTBEAT) + // buffer[3]: SYSID + // buffer[4]: COMPID + // buffer[5]: mavlink message id (0 for HEARTBEAT) + syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)\n", + USB_DEVICE_PATH, buffer[i + 3], buffer[i + 4]); + launch_mavlink = true; + break; + + } else if ((buffer[i] == 0xFD) && (buffer[i + 1] == 9) + && (buffer[i + 7] == 0) && (buffer[i + 8] == 0) && (buffer[i + 9] == 0)) { + // mavlink v2 HEARTBEAT + // buffer[0]: start byte (0xFD for mavlink v2) + // buffer[1]: length (9 for HEARTBEAT) + // buffer[5]: SYSID + // buffer[6]: COMPID + // buffer[7:9]: mavlink message id (0 for HEARTBEAT) + syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)\n", + USB_DEVICE_PATH, buffer[i + 5], buffer[i + 6]); + launch_mavlink = true; + break; + } + } + } + + if (!launch_mavlink && (nread >= 3)) { + // nshterm (3 carriage returns) + // scan buffer looking for 3 consecutive carriage returns (0xD) + for (int i = 1; i < nread - 1; i++) { + if (buffer[i - 1] == 0xD && buffer[i] == 0xD && buffer[i + 1] == 0xD) { + syslog(LOG_INFO, "%s: launching nshterm\n", USB_DEVICE_PATH); + launch_nshterm = true; + break; + } + } + } + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + + if (!launch_mavlink && !launch_nshterm && (nread >= 4)) { + // passthru Ublox + // scan buffer looking for 0xb5 0x62 + for (int i = 0; i < nread; i++) { + bool ub = buffer[i] == 0xb5 && buffer[i + 1] == 0x62; + + if (ub && ((buffer[i + 2 ] == 0x6 && (buffer[i + 3 ] == 0xb8 || buffer[i + 3 ] == 0x13)) || + (buffer[i + 2 ] == 0xa && buffer[i + 3 ] == 0x4))) { + syslog(LOG_INFO, "%s: launching serial_passthru\n", USB_DEVICE_PATH); + launch_passthru = true; + break; + } + } + } + +#endif + + if (launch_mavlink || launch_nshterm || launch_passthru) { + + // Get the current settings + tcgetattr(ttyacm_fd, &uart_config); + + // cleanup serial port + close(ttyacm_fd); + ttyacm_fd = -1; + + static const char *mavlink_argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, nullptr}; + static const char *nshterm_argv[] {"nshterm", USB_DEVICE_PATH, nullptr}; +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + speed_t baudrate = cfgetspeed(&uart_config); + char baudstring[16]; + snprintf(baudstring, sizeof(baudstring), "%d", baudrate); + static const char *gps_argv[] {"gps", "stop", nullptr}; + + static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV, nullptr}; +#endif + char **exec_argv = nullptr; + + if (launch_nshterm) { + exec_argv = (char **)nshterm_argv; + + } else if (launch_mavlink) { + exec_argv = (char **)mavlink_argv; + } + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + + else if (launch_passthru) { + sched_lock(); + exec_argv = (char **)gps_argv; + exec_builtin(exec_argv[0], exec_argv, nullptr, 0); + sched_unlock(); + exec_argv = (char **)passthru_argv; + } + +#endif + + sched_lock(); + + if (exec_builtin(exec_argv[0], exec_argv, nullptr, 0) > 0) { + usb_auto_start_state = UsbAutoStartState::connected; + + } else { + usb_auto_start_state = UsbAutoStartState::disconnecting; + } + + sched_unlock(); + } + } + } + } + + } else { + // cleanup + if (ttyacm_fd >= 0) { + close(ttyacm_fd); + ttyacm_fd = -1; + } + + usb_auto_start_state = UsbAutoStartState::disconnecting; + } + + break; + + case UsbAutoStartState::connected: + if (!vbus_present && !vbus_present_prev) { + sched_lock(); + static const char app[] {"mavlink"}; + static const char *stop_argv[] {"mavlink", "stop", "-d", USB_DEVICE_PATH, NULL}; + exec_builtin(app, (char **)stop_argv, NULL, 0); + sched_unlock(); + + usb_auto_start_state = UsbAutoStartState::disconnecting; + } + + break; + + case UsbAutoStartState::disconnecting: + // serial disconnect if unused + serdis_main(0, NULL); + usb_auto_start_state = UsbAutoStartState::disconnected; + break; + } + } + + vbus_present_prev = vbus_present; + + if (rescheduled != PX4_OK) { + work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, NULL, USEC2TICK(1000000)); + } +} + + +void cdcacm_init(void) +{ + work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, 0); +} + +#endif // CONFIG_SYSTEM_CDCACM diff --git a/platforms/nuttx/src/px4/common/console_buffer.cpp b/platforms/nuttx/src/px4/common/console_buffer.cpp index ae945f56a55c..c949421d2670 100644 --- a/platforms/nuttx/src/px4/common/console_buffer.cpp +++ b/platforms/nuttx/src/px4/common/console_buffer.cpp @@ -67,50 +67,47 @@ class ConsoleBuffer int _head{0}; int _tail{0}; px4_sem_t _lock = SEM_INITIALIZER(1); - bool _is_printing{false}; - pthread_t _printing_task; }; void ConsoleBuffer::print(bool follow) { - lock(); - _printing_task = pthread_self(); - int i = _head; + // print to stdout, but with a buffer in between to avoid nested locking and potential dead-locks + // (which could happen in combination with the MAVLink shell: dmesg writing to the pipe waiting + // mavlink to read, while mavlink calls printf, waiting for the console lock) + const int buffer_length = 512; + char buffer[buffer_length]; + int offset = -1; - while (true) { - _is_printing = true; + do { - if (i < _tail) { - ::write(1, _buffer + i, _tail - i); + // print the full buffer or what's newly added + int total_size_read = 0; - } else if (_tail < i) { - ::write(1, _buffer + i, BOARD_CONSOLE_BUFFER_SIZE - i); - ::write(1, _buffer, _tail); - } + do { + int read_size = read(buffer, buffer_length, &offset); + + if (read_size <= 0) { + break; + } - i = _tail; + ::write(1, buffer, read_size); + + if (read_size < buffer_length) { + break; + } + + total_size_read += read_size; + } while (total_size_read < BOARD_CONSOLE_BUFFER_SIZE); - _is_printing = false; if (follow) { - unlock(); usleep(10000); - lock(); - - } else { - break; } - } - - unlock(); + } while (follow); } void ConsoleBuffer::write(const char *buffer, size_t len) { - if (_is_printing && pthread_self() == _printing_task) { // avoid adding to the buffer while we are printing it - return; - } - lock(); // same rule as for printf: this cannot be used from IRQ handlers for (size_t i = 0; i < len; ++i) { diff --git a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp new file mode 100644 index 000000000000..83023f934d0e --- /dev/null +++ b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 +#include +#include +#include + +#ifdef BOARD_ENABLE_CONSOLE_BUFFER +#ifndef BOARD_CONSOLE_BUFFER_SIZE +# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size +#endif + + +// TODO: User side implementation of px4_console_buffer + +int px4_console_buffer_init() +{ + return 0; +} + +int px4_console_buffer_size() +{ + return 0; +} + +int px4_console_buffer_read(char *buffer, int buffer_length, int *offset) +{ + return 0; +} + +#endif /* BOARD_ENABLE_CONSOLE_BUFFER */ diff --git a/platforms/nuttx/src/px4/common/cpuload.cpp b/platforms/nuttx/src/px4/common/cpuload.cpp index 04582538c3e5..62c5c6205d0d 100644 --- a/platforms/nuttx/src/px4/common/cpuload.cpp +++ b/platforms/nuttx/src/px4/common/cpuload.cpp @@ -61,7 +61,7 @@ void cpuload_monitor_start() system_load.start_time = hrt_absolute_time(); - for (int i = 1; i < CONFIG_MAX_TASKS; i++) { + for (int i = 1; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { system_load.tasks[i].total_runtime = 0; system_load.tasks[i].curr_start_time = 0; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h new file mode 100644 index 000000000000..c3d9589758e9 --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. All rights reserved. + * Author: Jukka Laitinen + * + * 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 + +/* Encode the px4 boardctl ioctls in the following way: + * the highest 4-bits identifies the boardctl's used by this if + * the next 4-bits identifies the module which handles the ioctl + * the low byte identiefies the actual IOCTL within the module + */ + +#define _BOARDCTLIOCBASE (0x4000) +#define IOCTL_IDX_TO_BASE(x) ((((x) & 0xF) << 8) | _BOARDCTLIOCBASE) +#define IOCTL_BASE_TO_IDX(x) (((x) & 0x0F00) >> 8) + +#define _ORBIOCDEVBASE IOCTL_IDX_TO_BASE(0) +#define _HRTIOCBASE IOCTL_IDX_TO_BASE(1) +#define _CRYPTOIOCBASE IOCTL_IDX_TO_BASE(2) +#define _PARAMIOCBASE IOCTL_IDX_TO_BASE(3) +#define _PLATFORMIOCBASE IOCTL_IDX_TO_BASE(4) +#define MAX_IOCTL_PTRS 5 + +/* The PLATFORMIOCLAUNCH IOCTL is used to launch kernel side modules + * from the user side code + */ + +#define PLATFORMIOCLAUNCH (_PX4_IOC(_PLATFORMIOCBASE, 1)) + +typedef struct platformioclaunch { + int argc; + char **argv; + int ret; +} platformioclaunch_t; + +/* The PLATFORMIOCVBUSSTATE IOCTL is used to read USB VBUS state + * from the user side code + */ + +#define PLATFORMIOCVBUSSTATE (_PX4_IOC(_PLATFORMIOCBASE, 2)) + +typedef struct platformiocvbusstate { + int ret; +} platformiocvbusstate_t; + + +/* These IOCTLs are used to set and read external lockout state + * from the user side code + */ +#define PLATFORMIOCINDICATELOCKOUT (_PX4_IOC(_PLATFORMIOCBASE, 3)) +#define PLATFORMIOCGETLOCKOUT (_PX4_IOC(_PLATFORMIOCBASE, 4)) + +typedef struct platformioclockoutstate { + bool enabled; +} platformioclockoutstate_t; + + +typedef int (*ioctl_ptr_t)(unsigned int, unsigned long); + +__BEGIN_DECLS + +/* Function to initialize or reset the interface */ +void kernel_ioctl_initialize(void); + +/* Function to register a px4 boardctl handler */ +int px4_register_boardct_ioctl(unsigned base, ioctl_ptr_t func); + +__END_DECLS diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h b/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h index 2fc8bb80a388..56b7d3694501 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/cpuload.h @@ -48,7 +48,7 @@ struct system_load_taskinfo_s { struct system_load_s { uint64_t start_time{0}; - system_load_taskinfo_s tasks[CONFIG_MAX_TASKS] {}; + system_load_taskinfo_s tasks[CONFIG_FS_PROCFS_MAX_TASKS] {}; int total_count{0}; int running_count{0}; bool initialized{false}; diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h b/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h index cbfed2fa1157..f348b6017ecf 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/io_timer_init.h @@ -38,7 +38,7 @@ #include #include - +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) /** * Initialize the IO channel mapping from timer and channel configurations. * @param io_timers_conf configured timers @@ -58,6 +58,8 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co } uint32_t first_channel = UINT32_MAX; + uint32_t min_timer_channel = UINT32_MAX; + uint32_t max_timer_channel = 0; uint32_t channel_count = 0; for (uint32_t channel = 0; channel < MAX_TIMER_IO_CHANNELS; ++channel) { @@ -74,11 +76,23 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co } ++channel_count; + + if (timer_io_channels_conf[channel].timer_channel < min_timer_channel) { + min_timer_channel = timer_io_channels_conf[channel].timer_channel; + } + + if (timer_io_channels_conf[channel].timer_channel > max_timer_channel) { + max_timer_channel = timer_io_channels_conf[channel].timer_channel; + } } } if (first_channel == UINT32_MAX) { //unused timer, channel_count is 0 first_channel = 0; + + } else { + ret.element[i].lowest_timer_channel = min_timer_channel; + ret.element[i].channel_count_including_gaps = max_timer_channel - min_timer_channel + 1; } ret.element[i].first_channel_index = first_channel; @@ -99,3 +113,4 @@ static inline constexpr io_timers_channel_mapping_t initIOTimerChannelMapping(co return ret; } +#endif // DIRECT_PWM_OUTPUT_CHANNELS diff --git a/platforms/nuttx/src/px4/common/nuttx_random.c b/platforms/nuttx/src/px4/common/nuttx_random.c new file mode 100644 index 000000000000..26c2df3b4198 --- /dev/null +++ b/platforms/nuttx/src/px4/common/nuttx_random.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 + +#if defined(CONFIG_CRYPTO_RANDOM_POOL) +size_t px4_get_secure_random(uint8_t *out, + size_t outlen) +{ + /* TODO: can getrandom fail?? */ + arc4random_buf(out, outlen); + return outlen; +} +#else +#error CONFIG_CRYPTO_RANDOM_POOL has to be defined +#endif diff --git a/platforms/nuttx/src/px4/common/print_load.cpp b/platforms/nuttx/src/px4/common/print_load.cpp index ecfeb1fed0d1..fa341c7824a7 100644 --- a/platforms/nuttx/src/px4/common/print_load.cpp +++ b/platforms/nuttx/src/px4/common/print_load.cpp @@ -81,7 +81,7 @@ void init_print_load(struct print_load_s *s) s->last_times[0] = system_load.tasks[0].total_runtime; sched_unlock(); - for (int i = 1; i < CONFIG_MAX_TASKS; i++) { + for (int i = 1; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { s->last_times[i] = 0; } @@ -142,12 +142,12 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb float idle_load = 0.f; // create a copy of the runtimes because this could be updated during the print output - uint64_t total_runtime[CONFIG_MAX_TASKS] {}; + uint64_t total_runtime[CONFIG_FS_PROCFS_MAX_TASKS] {}; sched_lock(); print_state->new_time = hrt_absolute_time(); - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + for (int i = 0; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { if (system_load.tasks[i].valid) { total_runtime[i] = system_load.tasks[i].total_runtime; } @@ -182,7 +182,7 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb print_state->total_user_time = 0; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + for (int i = 0; i < CONFIG_FS_PROCFS_MAX_TASKS; i++) { sched_lock(); // need to lock the tcb access (but make it as short as possible) @@ -222,19 +222,16 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb uint8_t tcb_sched_priority = system_load.tasks[i].tcb->sched_priority; unsigned int tcb_num_used_fds = 0; // number of used file descriptors -#if CONFIG_NFILE_DESCRIPTORS > 0 - FAR struct task_group_s *group = system_load.tasks[i].tcb->group; + struct filelist *filelist = &system_load.tasks[i].tcb->group->tg_filelist; - if (group) { - for (int fd_index = 0; fd_index < CONFIG_NFILE_DESCRIPTORS; ++fd_index) { - if (group->tg_filelist.fl_files[fd_index].f_inode) { + for (int fdr = 0; fdr < filelist->fl_rows; fdr++) { + for (int fdc = 0; fdc < CONFIG_NFILE_DESCRIPTORS_PER_BLOCK; fdc++) { + if (filelist->fl_files[fdr][fdc].f_inode) { ++tcb_num_used_fds; } } } -#endif - sched_unlock(); switch (tcb_task_state) { @@ -335,11 +332,10 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb const float sched_load = 1.f - idle_load - task_load; - snprintf(buffer, buffer_length, "Processes: %d total, %d running, %d sleeping, max FDs: %d", + snprintf(buffer, buffer_length, "Processes: %d total, %d running, %d sleeping", system_load.total_count, print_state->running_count, - print_state->blocked_count, - CONFIG_NFILE_DESCRIPTORS); + print_state->blocked_count); cb(user); snprintf(buffer, buffer_length, "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle", (double)(task_load * 100.f), diff --git a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c index 178888b0965e..42e1e76f214f 100644 --- a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c +++ b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c @@ -14,7 +14,7 @@ * * Derived from drivers/mtd/m25px.c * - * Copyright (C) 2009-2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2009-2011, 2021 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without @@ -55,6 +55,7 @@ #include #include +#include #include #include #include @@ -270,7 +271,7 @@ void at24c_test(void) } } else if (result != 1) { - syslog(LOG_INFO, "unexpected %u\n", result); + syslog(LOG_INFO, "unexpected %zu\n", result); } if ((count % 100) == 0) { @@ -314,7 +315,7 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, #endif blocksleft = nblocks; - finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + finfo("startblock: %08jx nblocks: %zu\n", (intmax_t)startblock, nblocks); if (startblock >= priv->npages) { return 0; @@ -409,7 +410,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks = priv->npages - startblock; } - finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + finfo("startblock: %08jx nblocks: %zu\n", (intmax_t)startblock, nblocks); BOARD_EEPROM_WP_CTRL(false); @@ -505,7 +506,7 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) #endif ret = OK; - finfo("blocksize: %d erasesize: %d neraseblocks: %d\n", + finfo("blocksize: %" PRId32 " erasesize: %" PRId32 " neraseblocks: %" PRId32 "\n", geo->blocksize, geo->erasesize, geo->neraseblocks); } } @@ -566,9 +567,9 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev, priv->mtd.ioctl = at24c_ioctl; priv->dev = dev; - priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans"); - priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst"); - priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs"); + priv->perf_transfers = perf_alloc(PC_ELAPSED, "[at24c] eeprom transfer"); + priv->perf_resets_retries = perf_alloc(PC_COUNT, "[at24c] eeprom reset"); + priv->perf_errors = perf_alloc(PC_COUNT, "[at24c] eeprom errors"); } /* attempt to read to validate device is present */ @@ -599,6 +600,14 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev, perf_end(priv->perf_transfers); if (ret < 0) { + perf_free(priv->perf_transfers); + perf_free(priv->perf_resets_retries); + perf_free(priv->perf_errors); + + priv->perf_transfers = NULL; + priv->perf_resets_retries = NULL; + priv->perf_errors = NULL; + return NULL; } diff --git a/platforms/nuttx/src/px4/common/px4_crypto.cpp b/platforms/nuttx/src/px4/common/px4_crypto.cpp new file mode 100644 index 000000000000..6ae4e8f7ea88 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_crypto.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. 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 defined(PX4_CRYPTO) + +#include +#include + +extern "C" { +#include +} + + +px4_sem_t PX4Crypto::_lock; +bool PX4Crypto::_initialized = false; + +void PX4Crypto::px4_crypto_init() +{ + if (PX4Crypto::_initialized) { + return; + } + + px4_sem_init(&PX4Crypto::_lock, 0, 1); + + // Initialize nuttx random pool, if it is being used by crypto +#ifdef CONFIG_CRYPTO_RANDOM_POOL + up_randompool_initialize(); +#endif + + // initialize keystore functionality + keystore_init(); + + // initialize actual crypto algoritms + crypto_init(); + + PX4Crypto::_initialized = true; +} + +PX4Crypto::PX4Crypto() +{ + // Initialize an empty handle + crypto_session_handle_init(&_crypto_handle); +} + +PX4Crypto::~PX4Crypto() +{ + close(); +} + +bool PX4Crypto::open(px4_crypto_algorithm_t algorithm) +{ + bool ret = false; + lock(); + + // HW specific crypto already open? Just close before proceeding + if (crypto_session_handle_valid(_crypto_handle)) { + crypto_close(&_crypto_handle); + } + + // Open the HW specific crypto handle + _crypto_handle = crypto_open(algorithm); + + if (crypto_session_handle_valid(_crypto_handle)) { + ret = true; + } + + unlock(); + + return ret; +} + +void PX4Crypto::close() +{ + if (!crypto_session_handle_valid(_crypto_handle)) { + return; + } + + lock(); + crypto_close(&_crypto_handle); + unlock(); +} + +bool PX4Crypto::encrypt_data(uint8_t key_index, + const uint8_t *message, + size_t message_size, + uint8_t *cipher, + size_t *cipher_size) +{ + return crypto_encrypt_data(_crypto_handle, key_index, message, message_size, cipher, cipher_size); +} + +bool PX4Crypto::generate_key(uint8_t idx, + bool persistent) +{ + return crypto_generate_key(_crypto_handle, idx, persistent); +} + + +bool PX4Crypto::get_nonce(uint8_t *nonce, + size_t *nonce_len) +{ + return crypto_get_nonce(_crypto_handle, nonce, nonce_len); +} + + +bool PX4Crypto::get_encrypted_key(uint8_t key_idx, + uint8_t *key, + size_t *key_len, + uint8_t encryption_key_idx) +{ + return crypto_get_encrypted_key(_crypto_handle, key_idx, key, key_len, encryption_key_idx); +} + +size_t PX4Crypto::get_min_blocksize(uint8_t key_idx) +{ + return crypto_get_min_blocksize(_crypto_handle, key_idx); +} + +#endif diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 563974a171c6..08e2841869d8 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -44,56 +44,64 @@ #include -int px4_platform_console_init(void) -{ -#if !defined(CONFIG_DEV_CONSOLE) && defined(CONFIG_DEV_NULL) - - /* Support running nsh on a board with out a console - * Without this the assumption that the fd 0..2 are - * std{in..err} will be wrong. NSH will read/write to the - * fd it opens for the init script or nested scripts assigned - * to fd 0..2. - * - */ - - int fd = open("/dev/null", O_RDWR); - - if (fd == 0) { - /* Successfully opened /dev/null as stdin (fd == 0) */ - - (void)fs_dupfd2(0, 1); - (void)fs_dupfd2(0, 2); - (void)fs_fdopen(0, O_RDONLY, NULL, NULL); - (void)fs_fdopen(1, O_WROK | O_CREAT, NULL, NULL); - (void)fs_fdopen(2, O_WROK | O_CREAT, NULL, NULL); - - } else { - /* We failed to open /dev/null OR for some reason, we opened - * it and got some file descriptor other than 0. - */ - - if (fd > 0) { - (void)close(fd); - } +#include +#include - return -ENFILE; +#if defined(CONFIG_I2C) +# include +# include +#endif // CONFIG_I2C - } +#if defined(PX4_CRYPTO) +#include +#endif +#if !defined(CONFIG_BUILD_FLAT) +#include #endif - return OK; -} -int px4_platform_init(void) +extern void cdcacm_init(void); + +#if !defined(CONFIG_BUILD_FLAT) +typedef CODE void (*initializer_t)(void); +extern initializer_t _sinit; +extern initializer_t _einit; +extern uint32_t _stext; +extern uint32_t _etext; + +static void cxx_initialize(void) { + initializer_t *initp; - int ret = px4_platform_console_init(); + /* Visit each entry in the initialization table */ - if (ret < 0) { - return ret; + for (initp = &_sinit; initp != &_einit; initp++) { + initializer_t initializer = *initp; + + /* Make sure that the address is non-NULL and lies in the text + * region defined by the linker script. Some toolchains may put + * NULL values or counts in the initialization table. + */ + + if ((FAR void *)initializer >= (FAR void *)&_stext && + (FAR void *)initializer < (FAR void *)&_etext) { + initializer(); + } } +} +#endif - ret = px4_console_buffer_init(); +int px4_platform_init() +{ + +#if !defined(CONFIG_BUILD_FLAT) + cxx_initialize(); + + /* initialize userspace-kernelspace call gate interface */ + kernel_ioctl_initialize(); +#endif + + int ret = px4_console_buffer_init(); if (ret < 0) { return ret; @@ -108,6 +116,10 @@ int px4_platform_init(void) close(fd_buf); } +#if defined(PX4_CRYPTO) + PX4Crypto::px4_crypto_init(); +#endif + hrt_init(); param_init(); @@ -117,12 +129,63 @@ int px4_platform_init(void) cpuload_initialize_once(); #endif + +#if defined(CONFIG_I2C) + I2CBusIterator i2c_bus_iterator {I2CBusIterator::FilterType::All}; + + while (i2c_bus_iterator.next()) { + i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus().bus); + +#if defined(CONFIG_I2C_RESET) + I2C_RESET(i2c_dev); +#endif // CONFIG_I2C_RESET + + // send software reset to all + uint8_t buf[1] {}; + buf[0] = 0x06; // software reset + + i2c_msg_s msg{}; + msg.frequency = I2C_SPEED_STANDARD; + msg.addr = 0x00; // general call address + msg.buffer = &buf[0]; + msg.length = 1; + + I2C_TRANSFER(i2c_dev, &msg, 1); + + px4_i2cbus_uninitialize(i2c_dev); + } + +#endif // CONFIG_I2C + +#if defined(CONFIG_FS_PROCFS) + int ret_mount_procfs = mount(nullptr, "/proc", "procfs", 0, nullptr); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret_mount_procfs); + } + +#endif // CONFIG_FS_PROCFS + +#if defined(CONFIG_FS_BINFS) + int ret_mount_binfs = nx_mount(nullptr, "/bin", "binfs", 0, nullptr); + + if (ret_mount_binfs < 0) { + syslog(LOG_ERR, "ERROR: Failed to mount binfs at /bin: %d\n", ret_mount_binfs); + } + +#endif // CONFIG_FS_BINFS + + px4::WorkQueueManagerStart(); uorb_start(); px4_log_initialize(); +#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT) + cdcacm_init(); +#endif + return PX4_OK; } diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake new file mode 100644 index 000000000000..b138f05ddc35 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -0,0 +1,22 @@ +# Build the px4 layer for nuttx flat build + +add_library(px4_layer + ${KERNEL_SRCS} + cdc_acm_check.cpp + ) + +target_link_libraries(px4_layer + PRIVATE + ${KERNEL_LIBS} + nuttx_c + nuttx_arch + nuttx_mm + ) + + +if (DEFINED PX4_CRYPTO) + target_link_libraries(px4_layer + PUBLIC + crypto_backend + ) +endif() diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index ab0f48504e92..9fff4470ddda 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -49,6 +49,7 @@ #include #include +#include #include #include #include "systemlib/px4_macros.h" @@ -73,23 +74,27 @@ static int ramtron_attach(mtd_instance_s &instance) return ENXIO; #else - /* initialize the right spi */ - struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); + /* start the RAMTRON driver, attempt 10 times */ - if (spi == nullptr) { - PX4_ERR("failed to locate spi bus"); - return -ENXIO; - } + int spi_speed_mhz = 10; + + for (int i = 0; i < 10; i++) { + /* initialize the right spi */ + struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); - /* this resets the spi bus, set correct bus speed again */ - SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, instance.devid, false); + if (spi == nullptr) { + PX4_ERR("failed to locate spi bus"); + return -ENXIO; + } - /* start the RAMTRON driver, attempt 5 times */ + /* this resets the spi bus, set correct bus speed again */ + SPI_LOCK(spi, true); + SPI_SETFREQUENCY(spi, spi_speed_mhz * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, instance.devid, false); + SPI_LOCK(spi, false); - for (int i = 0; i < 5; i++) { instance.mtd_dev = ramtron_initialize(spi); if (instance.mtd_dev) { @@ -100,6 +105,10 @@ static int ramtron_attach(mtd_instance_s &instance) break; } + + // try reducing speed for next attempt + spi_speed_mhz--; + px4_usleep(10000); } /* if last attempt is still unsuccessful, abort */ @@ -108,7 +117,7 @@ static int ramtron_attach(mtd_instance_s &instance) return -EIO; } - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000); + int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)spi_speed_mhz * 1000 * 1000); if (ret != OK) { // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried @@ -363,7 +372,7 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd) char blockname[32]; - unsigned offset; + unsigned long offset; unsigned part; for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i].partition_block_counts[part], part++) { @@ -373,8 +382,8 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd) instances[i].part_dev[part] = mtd_partition(instances[i].mtd_dev, offset, instances[i].partition_block_counts[part]); if (instances[i].part_dev[part] == nullptr) { - PX4_ERR("mtd_partition failed. offset=%lu nblocks=%lu", - (unsigned long)offset, (unsigned long)nblocks); + PX4_ERR("mtd_partition failed. offset=%lu nblocks=%u", + offset, nblocks); rv = -ENOSPC; goto errout; } @@ -407,7 +416,7 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd) errout: if (rv < 0) { - PX4_ERR("mtd failure: %d bus %d address %d class %d", + PX4_ERR("mtd failure: %d bus %" PRId32 " address %" PRId32 " class %d", rv, PX4_I2C_DEVID_BUS(instances[i].devid), PX4_I2C_DEVID_ADDR(instances[i].devid), diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake new file mode 100644 index 000000000000..a3ab4a693177 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -0,0 +1,62 @@ + +# Build the user side px4_layer + +add_library(px4_layer + board_dma_alloc.c + board_fat_dma_alloc.c + tasks.cpp + console_buffer_usr.cpp + usr_mcu_version.cpp + cdc_acm_check.cpp + ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp + ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp + usr_hrt.cpp + px4_userspace_init.cpp +) + +target_link_libraries(px4_layer + PRIVATE + m + nuttx_c + nuttx_xx + nuttx_mm +) + +# Build the interface library between user and kernel side +add_library(px4_board_ctrl + board_ctrl.c +) + +add_dependencies(px4_board_ctrl nuttx_context px4_kernel_builtin_list_target) + +target_link_libraries(px4_layer + PUBLIC + board_bus_info +) + +# Build the kernel side px4_kernel_layer + +add_library(px4_kernel_layer + ${KERNEL_SRCS} +) + +target_link_libraries(px4_kernel_layer + PRIVATE + ${KERNEL_LIBS} + nuttx_kc + nuttx_karch + nuttx_kmm +) + +target_link_libraries(px4_kernel_layer + PUBLIC + board_bus_info +) + +if (DEFINED PX4_CRYPTO) + target_link_libraries(px4_kernel_layer PUBLIC crypto_backend) +endif() + +target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__) + +add_dependencies(px4_kernel_layer prebuild_targets) diff --git a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp new file mode 100644 index 000000000000..a776b501e93b --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. 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 px4_userspace_init.cpp + * + * Initialize px4 userspace in NuttX protected build + */ + +#include +#include +#include +#include +#include + +extern void cdcacm_init(void); + +extern "C" void px4_userspace_init(void) +{ + hrt_init(); + + px4::WorkQueueManagerStart(); + + uorb_start(); + +#if defined(CONFIG_SYSTEM_CDCACM) + cdcacm_init(); +#endif +} diff --git a/platforms/nuttx/src/px4/common/tasks.cpp b/platforms/nuttx/src/px4/common/tasks.cpp index 132afcb41ba1..9ad860dc182f 100644 --- a/platforms/nuttx/src/px4/common/tasks.cpp +++ b/platforms/nuttx/src/px4/common/tasks.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include @@ -67,8 +68,12 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_ clearenv(); #endif +#if !defined(__KERNEL__) /* create the task */ int pid = task_create(name, priority, stack_size, entry, argv); +#else + int pid = kthread_create(name, priority, stack_size, entry, argv); +#endif if (pid > 0) { /* configure the scheduler */ @@ -88,7 +93,7 @@ int px4_task_delete(int pid) const char *px4_get_taskname(void) { -#if CONFIG_TASK_NAME_SIZE > 0 +#if CONFIG_TASK_NAME_SIZE > 0 && (defined(__KERNEL__) || defined(CONFIG_BUILD_FLAT)) FAR struct tcb_s *thisproc = nxsched_self(); return thisproc->name; diff --git a/platforms/nuttx/src/px4/common/usr_hrt.cpp b/platforms/nuttx/src/px4/common/usr_hrt.cpp new file mode 100644 index 000000000000..d57712a32d45 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_hrt.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * 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 usr_hrt.c + * + * Userspace High-resolution timer callouts and timekeeping. + * + * This can be used with nuttx userspace + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +static px4_task_t g_usr_hrt_task = -1; + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + hrt_abstime abstime = 0; + boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime); + return abstime; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +void +hrt_store_absolute_time(volatile hrt_abstime *t) +{ + irqstate_t flags = px4_enter_critical_section(); + *t = hrt_absolute_time(); + px4_leave_critical_section(flags); +} + +/** + * Event dispatcher thread + */ +int +event_thread(int argc, char *argv[]) +{ + struct hrt_call *entry = NULL; + + while (1) { + /* Wait for hrt tick */ + boardctl(HRT_WAITEVENT, (uintptr_t)&entry); + + /* HRT event received, dispatch */ + if (entry) { + entry->usr_callout(entry->usr_arg); + } + } + + return 0; +} + +/** + * Request stop. + */ +bool hrt_request_stop() +{ + px4_task_delete(g_usr_hrt_task); + return true; +} + +/** + * Initialise the high-resolution timing module. + */ +void +hrt_init(void) +{ + px4_register_shutdown_hook(hrt_request_stop); + g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL); +} + +/** + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_boardctl_t ioc_parm; + ioc_parm.entry = entry; + ioc_parm.time = delay; + ioc_parm.callout = callout; + ioc_parm.arg = arg; + entry->usr_callout = callout; + entry->usr_arg = arg; + + boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm); +} + +/** + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_boardctl_t ioc_parm; + ioc_parm.entry = entry; + ioc_parm.time = calltime; + ioc_parm.interval = 0; + ioc_parm.callout = callout; + ioc_parm.arg = arg; + entry->usr_callout = callout; + entry->usr_arg = arg; + + boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm); +} + +/** + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_boardctl_t ioc_parm; + ioc_parm.entry = entry; + ioc_parm.time = delay; + ioc_parm.interval = interval; + ioc_parm.callout = callout; + ioc_parm.arg = arg; + entry->usr_callout = callout; + entry->usr_arg = arg; + + boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm); +} + +/** + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + boardctl(HRT_CANCEL, (uintptr_t)entry); +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +/** + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +latency_info_t +get_latency(uint16_t bucket_idx, uint16_t counter_idx) +{ + latency_boardctl_t latency_ioc; + latency_ioc.bucket_idx = bucket_idx; + latency_ioc.counter_idx = counter_idx; + latency_ioc.latency = {0, 0}; + boardctl(HRT_GET_LATENCY, (uintptr_t)&latency_ioc); + return latency_ioc.latency; +} + +void reset_latency_counters() +{ + boardctl(HRT_RESET_LATENCY, NULL); +} diff --git a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp new file mode 100644 index 000000000000..0928bb3a1077 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. All rights reserved. + * Author: @author Jukka Laitinen + * + * 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 usr_mcu_version.c + * Implementation of generic user-space version API + */ + +#include +#include + +#include "board_config.h" + +static int hw_version = 0; +static int hw_revision = 0; +static char hw_info[] = HW_INFO_INIT; + +__EXPORT const char *board_get_hw_type_name(void) +{ + return (const char *) hw_info; +} + +__EXPORT int board_get_hw_version(void) +{ + return hw_version; +} + +__EXPORT int board_get_hw_revision() +{ + return hw_revision; +} + +__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the uuid32 from the kernel + */ + uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH]; + memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4); + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uuid_words[i] = chip_uuid[i]; + } +} + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the version from the kernel + */ + return -1; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + /* TODO: This is a stub for userspace build. Use some proper interface + * to fetch the guid from the kernel + */ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + memset(pb, 0, PX4_GUID_BYTE_LENGTH); + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} + diff --git a/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp index 8f9513032145..be8992e516cf 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/adc/adc.cpp @@ -91,14 +91,14 @@ int px4_arch_adc_init(uint32_t base_address) once = true; - /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + /* Input is Buss Clock 144 Mhz We will use /4 for 36 Mhz */ irqstate_t flags = px4_enter_critical_section(); imxrt_clockall_adc1(); - rCFG(base_address) = ADC_CFG_ADICLK_IPGDIV2 | ADC_CFG_MODE_12BIT | \ - ADC_CFG_ADIV_DIV8 | ADC_CFG_ADLSMP | ADC_CFG_ADSTS_7_21 | \ + rCFG(base_address) = ADC_CFG_ADICLK_IPG | ADC_CFG_MODE_12BIT | \ + ADC_CFG_ADIV_DIV4 | ADC_CFG_ADLSMP | ADC_CFG_ADSTS_7_21 | \ ADC_CFG_AVGS_4SMPL | ADC_CFG_OVWREN; px4_leave_critical_section(flags); @@ -156,6 +156,7 @@ int px4_arch_adc_init(uint32_t base_address) return 0; } + void px4_arch_adc_uninit(uint32_t base_address) { imxrt_clockoff_adc1(); @@ -164,6 +165,8 @@ void px4_arch_adc_uninit(uint32_t base_address) uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) { + irqstate_t flags = px4_enter_critical_section(); + /* clear any previous COCO0 */ uint16_t result = rR0(base_address); @@ -174,16 +177,19 @@ uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) hrt_abstime now = hrt_absolute_time(); while (!(rHS(base_address) & ADC_HS_COCO0)) { - /* don't wait for more than 50us, since that means something broke + /* don't wait for more than 10us, since that means something broke * should reset here if we see this */ - if ((hrt_absolute_time() - now) > 50) { - return 0xffff; + if ((hrt_absolute_time() - now) > 30) { + px4_leave_critical_section(flags); + return UINT32_MAX; } } /* read the result and clear COCO0 */ result = rR0(base_address); + px4_leave_critical_section(flags); + return result; } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c index f0097cb96799..d4768fdd1eb1 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -160,7 +160,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Turn the sense lines to digital outputs LOW */ - imxrt_config_gpio(PX4_MAKE_GPIO_OUTPUT(gpio_sense)); + imxrt_config_gpio(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense)); up_udelay(100); /* About 10 TC assuming 485 K */ @@ -172,7 +172,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Write the sense lines HIGH */ - imxrt_gpio_write(PX4_MAKE_GPIO_OUTPUT(gpio_sense), 1); + imxrt_gpio_write(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense), 1); up_udelay(100); /* About 10 TC assuming 485 K */ 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 f2716046102a..06ccd082037c 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt @@ -34,3 +34,10 @@ px4_add_library(arch_board_reset board_reset.cpp ) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c index c5864f17e0e5..7106a1814cbf 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c @@ -561,46 +561,6 @@ hrt_absolute_time(void) return abstime; } -/** - * Convert a timespec to absolute time - */ -hrt_abstime -ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/** - * Convert absolute time to a timespec. - */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - -/** - * Compare a time value with the current time as atomic operation. - */ -hrt_abstime -hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime delta = hrt_absolute_time() - *then; - - px4_leave_critical_section(flags); - - return delta; -} - /** * Store the absolute time in an interrupt-safe fashion */ diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/i2c_hw_description.h index 2e9e55f44355..4b7940e1dd1b 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/i2c_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/i2c_hw_description.h @@ -36,6 +36,7 @@ #include #include +#if defined(CONFIG_I2C) static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) { @@ -52,3 +53,4 @@ static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) ret.is_external = true; return ret; } +#endif // CONFIG_I2C 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 8d55042f75ed..3efb8101b0b9 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 @@ -60,6 +60,10 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Capture = 3, IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -82,6 +86,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ @@ -98,6 +104,7 @@ typedef struct timer_io_channels_t { uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */ uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ + uint8_t timer_channel; /* Unused */ } timer_io_channels_t; #define SM0 0 @@ -123,26 +130,31 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; -__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; - __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context); -__EXPORT int io_timer_init_timer(unsigned timer); +__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate); __EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); __EXPORT uint16_t io_channel_get_ccr(unsigned channel); __EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); __EXPORT uint32_t io_timer_get_group(unsigned timer); __EXPORT int io_timer_validate_channel_index(unsigned channel); -__EXPORT int io_timer_is_channel_free(unsigned channel); -__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode); +__EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); + +/** + * Reserve a timer + * @return 0 on success (if not used yet, or already set to the mode) + */ +__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode); + +__EXPORT int io_timer_unallocate_timer(unsigned timer); /** * Returns the pin configuration for a specific channel, to be used as GPIO output. diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/spi_hw_description.h index 91bc32e9f5d4..4d069039f1cc 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/spi_hw_description.h @@ -36,6 +36,8 @@ #include #include +#if defined(CONFIG_SPI) + #include #define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST) @@ -73,16 +75,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { ret.devices[i] = devices.devices[i]; - // check that the same device is configured only once (the chip-select code depends on that) - for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) { - if (ret.devices[j].cs_gpio != 0) { - constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times"); - } - } if (ret.devices[i].cs_gpio != 0) { - // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) - if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + int same_devices_count = 0; + + for (int j = 0; j < i; ++j) { + if (ret.devices[j].cs_gpio != 0) { + same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff); + } + } + + // increment the 2. LSB byte to allow multiple devices of the same type + ret.devices[i].devid |= same_devices_count << 8; + + } else { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) ret.requires_locking = true; } } @@ -129,3 +137,4 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI ret.drdy_gpio = drdy_gpio; return ret; } +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c index a5b0e3ce4cb6..6fb285c5831f 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c @@ -87,11 +87,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, { channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = 0;//_REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF; @@ -132,7 +132,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt int rv = io_timer_validate_channel_index(channel); if (rv == 0) { - if (edge == Disabled) { io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); @@ -140,10 +139,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } else { - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - input_capture_bind(channel, callback, context); rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index e7de2d859f11..b91f011373e1 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -149,12 +149,14 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define rFCTRL20(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL20_OFFSET) /* Fault Control 2 Register */ -// NotUsed PWMOut PWMIn Capture OneShot Trigger -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ -static io_timer_allocation_t once = 0; +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; + +/* Stats and handlers are only useful for Capture */ typedef struct channel_stat_t { uint32_t isr_cout; @@ -213,22 +215,40 @@ int io_timer_handler7(int irq, void *context, void *arg) return io_timer_handler(7); } -static inline int is_timer_uninitalized(unsigned timer) +static inline int validate_timer_index(unsigned timer) { - int rv = 0; + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) +{ + int ret = -EINVAL; + + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; - if (once & 1 << timer) { - rv = -EBUSY; + } else { + ret = -EBUSY; + } } - return rv; + return ret; } -static inline void set_timer_initalized(unsigned timer) +int io_timer_unallocate_timer(unsigned timer) { - once |= 1 << timer; -} + int ret = -EINVAL; + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; +} static inline int channels_timer(unsigned channel) { @@ -240,19 +260,6 @@ static uint32_t get_channel_mask(unsigned channel) return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0; } -int io_timer_is_channel_free(unsigned channel) -{ - int rv = io_timer_validate_channel_index(channel); - - if (rv == 0) { - if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { - rv = -EBUSY; - } - } - - return rv; -} - int io_timer_validate_channel_index(unsigned channel) { int rv = -EINVAL; @@ -337,21 +344,26 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode return before ^ channels; } -static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { - int rv = io_timer_is_channel_free(channel); + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; - if (rv == 0) { + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; + ret = 0; } - return rv; + px4_leave_critical_section(flags); + + return ret; } -static inline int free_channel_resource(unsigned channel) +int io_timer_unallocate_channel(unsigned channel) { int mode = io_timer_get_channel_mode(channel); @@ -364,24 +376,6 @@ static inline int free_channel_resource(unsigned channel) return mode; } -int io_timer_free_channel(unsigned channel) -{ - if (io_timer_validate_channel_index(channel) != 0) { - return -EINVAL; - } - - int mode = io_timer_get_channel_mode(channel); - - if (mode > IOTimerChanMode_NotUsed) { - io_timer_set_enable(false, mode, 1 << channel); - free_channel_resource(channel); - - } - - return 0; -} - - static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { int rv = -EINVAL; @@ -390,7 +384,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) rv = io_timer_validate_channel_index(channel); if (rv == 0) { - rv = allocate_channel_resource(channel, mode); + rv = io_timer_allocate_channel(channel, mode); } } @@ -435,9 +429,9 @@ static inline void io_timer_set_PWM_mode(unsigned channel) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; struct { uint32_t base; uint16_t triggers; @@ -479,18 +473,20 @@ void io_timer_trigger(void) px4_leave_critical_section(flags); } -int io_timer_init_timer(unsigned timer) +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) { - /* Do this only once per timer */ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } - int rv = is_timer_uninitalized(timer); + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); - if (rv == 0) { + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { irqstate_t flags = px4_enter_critical_section(); - set_timer_initalized(timer); - /* enable the timer clock before we try to talk to it */ switch (io_timers[timer].base) { @@ -545,62 +541,54 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned channel, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the channel */ - - uint32_t channels = get_channel_mask(channel); - - /* Check that all channels are either in PWM or Oneshot */ - - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } - /* Change only a timer that is owned by pwm or one shot */ + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ - /* Request to use OneShot ?*/ + uint32_t channels = get_channel_mask(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); - if (rate == 0) { + /* Request to use OneShot ?*/ - /* Request to use OneShot - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be Oneshot - */ + if (PWM_RATE_ONESHOT == rate) { - /* Did the allocation change */ - if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { - io_timer_set_oneshot_mode(channel); - } + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - } else { + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } - /* Request to use PWM - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be PWM - */ + } else { - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { - io_timer_set_PWM_mode(channel); - } + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - timer_set_rate(channel, rate); + if (changed_channels) { + io_timer_set_PWM_mode(timer); } - rv = OK; + timer_set_rate(timer, rate); } - return rv; + return OK; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + uint32_t gpio = 0; /* figure out the GPIO config first */ @@ -625,19 +613,26 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, return -EINVAL; } - int rv = allocate_channel(channel, mode); + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config - /* Valid channel should now be reserved in new mode */ + int previous_mode = io_timer_get_channel_mode(channel); + int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); - if (rv >= 0) { + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ - unsigned timer = channels_timer(channel); + rv = io_timer_init_timer(timer, mode); - /* Blindly try to initialize the timer - it will only do it once */ + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } - io_timer_init_timer(timer); + /* Valid channel should now be reserved in new mode */ - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { @@ -650,9 +645,10 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; - px4_leave_critical_section(flags); } + px4_leave_critical_section(flags); + return rv; } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c index 3af467fac3e6..07edc7f3341d 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c @@ -74,40 +74,50 @@ servo_position_t up_pwm_servo_get(unsigned channel) int up_pwm_servo_init(uint32_t channel_mask) { /* Init channels */ - uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot); // First free the current set of PWMs for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (current & (1 << channel)) { - io_timer_free_channel(channel); + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); current &= ~(1 << channel); } } - // Now allocate the new set + + /* Now allocate the new set */ + + int ret_val = OK; + int channels_init_mask = 0; for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (channel_mask & (1 << channel)) { // First free any that were not PWM mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } -void up_pwm_servo_deinit(void) +void up_pwm_servo_deinit(uint32_t channel_mask) { /* disable the timers */ - up_pwm_servo_arm(false); + up_pwm_servo_arm(false, channel_mask); } int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate) @@ -131,31 +141,25 @@ int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate) } } - return io_timer_set_rate(channel, rate); -} - -void up_pwm_update(void) -{ - io_timer_trigger(); + return io_timer_set_pwm_rate(channel, rate); } -int up_pwm_servo_set_rate(unsigned rate) +void up_pwm_update(unsigned channel_mask) { - for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; i++) { - up_pwm_servo_set_rate_group_update(i, rate); - } - - return 0; + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) { - return io_timer_get_group(group); + /* only return the set of channels in the group which we own */ + return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot)) & + io_timer_get_group(group); } void -up_pwm_servo_arm(bool armed) +up_pwm_servo_arm(bool armed, uint32_t channel_mask) { - io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS); - io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c index 7e9869d0d3e2..4f2d6d975025 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c @@ -64,23 +64,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value) int up_pwm_trigger_init(uint32_t channel_mask) { /* Init channels */ - for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { + int ret_val = OK; + int channels_init_mask = 0; - // First free any that were not trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { - io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } /* Enable the timers */ - up_pwm_trigger_arm(true); + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_trigger_deinit() diff --git a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h index 0a870b6a96ff..58d4421ba669 100644 --- a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h @@ -87,10 +87,6 @@ __BEGIN_DECLS #define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) #define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) -#define kinetis_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet - -#define px4_savepanic(fileno, context, length) kinetis_bbsram_savepanic(fileno, context, length) - /* bus_num is zero based on kinetis and must be translated from the legacy one based */ #define PX4_BUS_OFFSET 1 /* Kinetis buses are 0 based and adjustment is needed */ @@ -113,6 +109,8 @@ int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, boo #define _PX4_MAKE_GPIO(pin_ftmx, io) ((((uint32_t)(pin_ftmx)) & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) |(io)) #define PX4_MAKE_GPIO_INPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_PULLUP) -#define PX4_MAKE_GPIO_OUTPUT(gpio) _PX4_MAKE_GPIO(gpio, GPIO_HIGHDRIVE) +#define PX4_MAKE_GPIO_EXTI(gpio) _PX4_MAKE_GPIO(gpio, PIN_INT_BOTH | GPIO_PULLUP) +#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) _PX4_MAKE_GPIO(gpio, GPIO_HIGHDRIVE) +#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) _PX4_MAKE_GPIO(gpio, GPIO_LOWDRIVE) __END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/spi_hw_description.h index 4b5010d2f676..c62f513e146f 100644 --- a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/spi_hw_description.h @@ -32,9 +32,12 @@ ****************************************************************************/ #pragma once +#if defined(CONFIG_SPI) + #include "../../../kinetis/include/px4_arch/spi_hw_description.h" constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) { return true; } +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/CMakeLists.txt index f2716046102a..06ccd082037c 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/kinetis/board_reset/CMakeLists.txt @@ -34,3 +34,10 @@ px4_add_library(arch_board_reset board_reset.cpp ) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c index 7825271d4b8a..8956ddf15a1c 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c @@ -573,46 +573,6 @@ hrt_absolute_time(void) return abstime; } -/** - * Convert a timespec to absolute time - */ -hrt_abstime -ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/** - * Convert absolute time to a timespec. - */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - -/** - * Compare a time value with the current time as atomic operation. - */ -hrt_abstime -hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime delta = hrt_absolute_time() - *then; - - px4_leave_critical_section(flags); - - return delta; -} - /** * Store the absolute time in an interrupt-safe fashion */ diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/i2c_hw_description.h index 2e9e55f44355..4b7940e1dd1b 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/i2c_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/i2c_hw_description.h @@ -36,6 +36,7 @@ #include #include +#if defined(CONFIG_I2C) static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) { @@ -52,3 +53,4 @@ static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) ret.is_external = true; return ret; } +#endif // CONFIG_I2C 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 a1e35446b811..f6cd7427bec6 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 @@ -64,6 +64,10 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Capture = 3, IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -87,6 +91,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ @@ -122,21 +128,29 @@ __EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context); -__EXPORT int io_timer_init_timer(unsigned timer); +__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate); __EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); __EXPORT uint16_t io_channel_get_ccr(unsigned channel); __EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); __EXPORT uint32_t io_timer_get_group(unsigned timer); __EXPORT int io_timer_validate_channel_index(unsigned channel); -__EXPORT int io_timer_is_channel_free(unsigned channel); -__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode); +__EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); + +/** + * Reserve a timer + * @return 0 on success (if not used yet, or already set to the mode) + */ +__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode); + +__EXPORT int io_timer_unallocate_timer(unsigned timer); + /** * Returns the pin configuration for a specific channel, to be used as GPIO output. * 0 is returned if the channel is not valid. @@ -148,5 +162,4 @@ __EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); */ __EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); - __END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/spi_hw_description.h index 5dda16ec168e..835634f8e79e 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/spi_hw_description.h @@ -36,6 +36,8 @@ #include #include +#if defined(CONFIG_SPI) + #include static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) @@ -67,16 +69,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { ret.devices[i] = devices.devices[i]; - // check that the same device is configured only once (the chip-select code depends on that) - for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) { - if (ret.devices[j].cs_gpio != 0) { - constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times"); - } - } if (ret.devices[i].cs_gpio != 0) { - // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) - if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + int same_devices_count = 0; + + for (int j = 0; j < i; ++j) { + if (ret.devices[j].cs_gpio != 0) { + same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff); + } + } + + // increment the 2. LSB byte to allow multiple devices of the same type + ret.devices[i].devid |= same_devices_count << 8; + + } else { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) ret.requires_locking = true; } } @@ -123,3 +131,5 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI ret.drdy_gpio = drdy_gpio; return ret; } + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c index a1e349077181..69f3a3c0a351 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c @@ -104,11 +104,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, { channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = _REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF; @@ -169,10 +169,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } else { - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - input_capture_bind(channel, callback, context); rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 393920d39d12..dd5968db2957 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -152,12 +152,14 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define CnSC_PWMIN_INIT 0 // TBD -// NotUsed PWMOut PWMIn Capture OneShot Trigger -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ -static io_timer_allocation_t once = 0; +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; + +/* Stats and handlers are only useful for Capture */ typedef struct channel_stat_t { uint32_t isr_cout; @@ -269,25 +271,34 @@ static inline int validate_timer_index(unsigned timer) return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; } -static inline int is_timer_uninitalized(unsigned timer) +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) { - int rv = 0; + int ret = -EINVAL; + + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; - if (once & 1 << timer) { - rv = -EBUSY; + } else { + ret = -EBUSY; + } } - return rv; + return ret; } -static inline void set_timer_initalized(unsigned timer) +int io_timer_unallocate_timer(unsigned timer) { - once |= 1 << timer; -} + int ret = -EINVAL; -static inline void set_timer_deinitalized(unsigned timer) -{ - once &= ~(1 << timer); + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; } static inline int channels_timer(unsigned channel) @@ -324,24 +335,6 @@ static uint32_t get_timer_channels(unsigned timer) return channels_cache[timer]; } -static inline int is_channels_timer_uninitalized(unsigned channel) -{ - return is_timer_uninitalized(channels_timer(channel)); -} - -int io_timer_is_channel_free(unsigned channel) -{ - int rv = io_timer_validate_channel_index(channel); - - if (rv == 0) { - if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { - rv = -EBUSY; - } - } - - return rv; -} - int io_timer_validate_channel_index(unsigned channel) { int rv = -EINVAL; @@ -380,7 +373,6 @@ uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) return timer_io_channels[channel].gpio_in; } - int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { @@ -429,21 +421,26 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode return before ^ channels; } -static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { - int rv = io_timer_is_channel_free(channel); + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; - if (rv == 0) { + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; + ret = 0; } - return rv; + px4_leave_critical_section(flags); + + return ret; } -static inline int free_channel_resource(unsigned channel) +int io_timer_unallocate_channel(unsigned channel) { int mode = io_timer_get_channel_mode(channel); @@ -456,24 +453,6 @@ static inline int free_channel_resource(unsigned channel) return mode; } -int io_timer_free_channel(unsigned channel) -{ - if (io_timer_validate_channel_index(channel) != 0) { - return -EINVAL; - } - - int mode = io_timer_get_channel_mode(channel); - - if (mode > IOTimerChanMode_NotUsed) { - io_timer_set_enable(false, mode, 1 << channel); - free_channel_resource(channel); - - } - - return 0; -} - - static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { int rv = -EINVAL; @@ -482,7 +461,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) rv = io_timer_validate_channel_index(channel); if (rv == 0) { - rv = allocate_channel_resource(channel, mode); + rv = io_timer_allocate_channel(channel, mode); } } @@ -539,9 +518,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; uint32_t action_cache[MAX_IO_TIMERS] = {0}; int actions = 0; @@ -568,18 +547,20 @@ void io_timer_trigger(void) px4_leave_critical_section(flags); } -int io_timer_init_timer(unsigned timer) +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) { - /* Do this only once per timer */ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } - int rv = is_timer_uninitalized(timer); + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); - if (rv == 0) { + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { irqstate_t flags = px4_enter_critical_section(); - set_timer_initalized(timer); - /* enable the timer clock before we try to talk to it */ uint32_t regval = _REG(io_timers[timer].clock_register); @@ -650,62 +631,54 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned timer, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the timer */ - - uint32_t channels = get_timer_channels(timer); - - /* Check that all channels are either in PWM or Oneshot */ + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ - /* Change only a timer that is owned by pwm or one shot */ + uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); - /* Request to use OneShot ?*/ + /* Request to use OneShot ?*/ - if (rate == 0) { + if (PWM_RATE_ONESHOT == rate) { - /* Request to use OneShot - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be Oneshot - */ - - /* Did the allocation change */ - if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { - io_timer_set_oneshot_mode(timer); - } + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - } else { + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } - /* Request to use PWM - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be PWM - */ + } else { - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { - io_timer_set_PWM_mode(timer); - } + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - timer_set_rate(timer, rate); + if (changed_channels) { + io_timer_set_PWM_mode(timer); } - rv = OK; + timer_set_rate(timer, rate); } - return rv; + return OK; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + uint32_t gpio = timer_io_channels[channel].gpio_in; uint32_t clearbits = CnSC_RESET; uint32_t setbits = CnSC_CAPTURE_INIT; @@ -733,25 +706,33 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, return -EINVAL; } + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config + + int previous_mode = io_timer_get_channel_mode(channel); int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); - /* Valid channel should now be reserved in new mode */ + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ - if (rv >= 0) { + rv = io_timer_init_timer(timer, mode); - /* Blindly try to initialize the timer - it will only do it once */ + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } - io_timer_init_timer(channels_timer(channel)); + /* Valid channel should now be reserved in new mode */ - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { px4_arch_configgpio(gpio); } - - unsigned timer = channels_timer(channel); + /* configure the channel */ /* configure the channel */ @@ -767,9 +748,10 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; - px4_leave_critical_section(flags); } + px4_leave_critical_section(flags); + return rv; } diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/kinetis_pinirq.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/kinetis_pinirq.c index 614120095111..70ff0cf393cd 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/kinetis_pinirq.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/kinetis_pinirq.c @@ -71,8 +71,7 @@ int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, } else { ret = kinetis_pinirqattach(pinset, func, arg); - pinset &= ~_PIN_INT_MASK - DEBUGASSERT(port < KINETIS_NPORTS); + pinset &= ~_PIN_INT_MASK; if (risingedge) { pinset |= PIN_INT_RISING; diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c index 9534f51df33f..408fbb37f11e 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c @@ -75,40 +75,48 @@ servo_position_t up_pwm_servo_get(unsigned channel) int up_pwm_servo_init(uint32_t channel_mask) { /* Init channels */ - uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot); // First free the current set of PWMs for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (current & (1 << channel)) { - io_timer_free_channel(channel); + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); current &= ~(1 << channel); } } - // Now allocate the new set + + /* Now allocate the new set */ + + int ret_val = OK; + int channels_init_mask = 0; for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (channel_mask & (1 << channel)) { - // First free any that were not PWM mode before + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; } - - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); - channel_mask &= ~(1 << channel); } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } -void up_pwm_servo_deinit(void) +void up_pwm_servo_deinit(uint32_t channel_mask) { /* disable the timers */ - up_pwm_servo_arm(false); + up_pwm_servo_arm(false, channel_mask); } int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) @@ -132,31 +140,25 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) } } - return io_timer_set_rate(group, rate); + return io_timer_set_pwm_rate(group, rate); } -void up_pwm_update(void) +void up_pwm_update(unsigned channel_mask) { - io_timer_trigger(); -} - -int up_pwm_servo_set_rate(unsigned rate) -{ - for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { - up_pwm_servo_set_rate_group_update(i, rate); - } - - return 0; + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) { - return io_timer_get_group(group); + /* only return the set of channels in the group which we own */ + return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot)) & + io_timer_get_group(group); } void -up_pwm_servo_arm(bool armed) +up_pwm_servo_arm(bool armed, uint32_t channel_mask) { - io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS); - io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); } diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c index f2f002aa7527..5386919d388f 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c @@ -64,23 +64,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value) int up_pwm_trigger_init(uint32_t channel_mask) { /* Init channels */ - for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { + int ret_val = OK; + int channels_init_mask = 0; - // First free any that were not trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { - io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } /* Enable the timers */ - up_pwm_trigger_arm(true); + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_trigger_deinit() diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h index 368271913cf2..bd2719405b5c 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h @@ -82,10 +82,6 @@ __BEGIN_DECLS #define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) #define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) -#define imxrt_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet - -#define px4_savepanic(fileno, context, length) imxrt_bbsram_savepanic(fileno, context, length) - /* bus_num is 1 based on imx and must be translated from the legacy one based */ #define PX4_BUS_OFFSET 0 /* imxrt buses are 1 based no adjustment needed */ @@ -105,6 +101,7 @@ int imxrt_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) imxrt_gpiosetevent(pinset,r,f,e,fp,a) #define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ)) -#define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)) +#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)) +#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ONE | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)) __END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/spi_hw_description.h index 78c5596b569b..b736695d8551 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/spi_hw_description.h @@ -32,9 +32,12 @@ ****************************************************************************/ #pragma once +#if defined(CONFIG_SPI) + #include "../../../imxrt/include/px4_arch/spi_hw_description.h" constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) { return true; } +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..9f782dcb24d1 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/i2c_hw_description.h @@ -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. + * + ****************************************************************************/ +#pragma once + +#include +#include + +#if defined(CONFIG_I2C) + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} +#endif // CONFIG_I2C diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h index f5c3c4fddbef..14c88d609d76 100644 --- a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/micro_hal.h @@ -89,10 +89,6 @@ __BEGIN_DECLS #define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) #define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) -#define s32k1xx_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet - -#define px4_savepanic(fileno, context, length) s32k1xx_bbsram_savepanic(fileno, context, length) - /* bus_num is zero based on s32k1xx and must be translated from the legacy one based */ #define PX4_BUS_OFFSET 1 /* s32k1xx buses are 0 based and adjustment is needed */ diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..ac9cf79c5993 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/spi_hw_description.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 + +#include "../../../s32k1xx/include/px4_arch/spi_hw_description.h" + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { +#ifdef CONFIG_S32K1XX_LPSPI0 + true, +#else + false, +#endif +#ifdef CONFIG_S32K1XX_LPSPI1 + true, +#else + false, +#endif +#ifdef CONFIG_S32K1XX_LPSPI2 + true, +#else + false, +#endif + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_S32K1XX_LPSPIn)"); + } + + return false; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt index f2716046102a..06ccd082037c 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/CMakeLists.txt @@ -34,3 +34,10 @@ px4_add_library(arch_board_reset board_reset.cpp ) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c index 5c76433d0223..71a97c9d819d 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c @@ -609,46 +609,6 @@ hrt_absolute_time(void) return abstime; } -/** - * Convert a timespec to absolute time - */ -hrt_abstime -ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/** - * Convert absolute time to a timespec. - */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - -/** - * Compare a time value with the current time as atomic operation. - */ -hrt_abstime -hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime delta = hrt_absolute_time() - *then; - - px4_leave_critical_section(flags); - - return delta; -} - /** * Store the absolute time in an interrupt-safe fashion */ diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/hw_description.h index 48141093c182..122a81aa0847 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/hw_description.h @@ -240,3 +240,22 @@ static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin) return 0; } + +namespace SPI +{ + +enum class Bus { + SPI0 = 1, + SPI1, + SPI2, +}; + +using CS = GPIO::GPIOPin; +using DRDY = GPIO::GPIOPin; + +struct bus_device_external_cfg_t { + CS cs_gpio; + DRDY drdy_gpio; +}; + +} // namespace SPI diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..9f782dcb24d1 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/i2c_hw_description.h @@ -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. + * + ****************************************************************************/ +#pragma once + +#include +#include + +#if defined(CONFIG_I2C) + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} +#endif // CONFIG_I2C 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 a5c449a4a40f..71a026b5c519 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 @@ -60,6 +60,10 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Capture = 3, IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -83,6 +87,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ @@ -98,7 +104,6 @@ typedef struct timer_io_channels_t { uint8_t timer_channel; /* 1 based channel index GPIO_FTMt_CHcIN = c+1) */ } timer_io_channels_t; - typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index, const timer_io_channels_t *chan, hrt_abstime isrs_time, uint16_t isrs_rcnt, @@ -113,26 +118,32 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; -__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; - __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context); -__EXPORT int io_timer_init_timer(unsigned timer); +__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate); __EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); __EXPORT uint16_t io_channel_get_ccr(unsigned channel); __EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); __EXPORT uint32_t io_timer_get_group(unsigned timer); __EXPORT int io_timer_validate_channel_index(unsigned channel); -__EXPORT int io_timer_is_channel_free(unsigned channel); -__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode); +__EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channel_mask); + +/** + * Reserve a timer + * @return 0 on success (if not used yet, or already set to the mode) + */ +__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode); + +__EXPORT int io_timer_unallocate_timer(unsigned timer); + /** * Returns the pin configuration for a specific channel, to be used as GPIO output. * 0 is returned if the channel is not valid. @@ -144,5 +155,4 @@ __EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); */ __EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); - __END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..cc1f42554a1f --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/spi_hw_description.h @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include "s32k1xx_config.h" +#include "s32k1xx_lpspi.h" +#include "s32k1xx_pin.h" + +static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + px4_spi_bus_device_t ret{}; + ret.cs_gpio = getGPIOPort(cs_gpio.port) | getGPIOPin(cs_gpio.pin) | (GPIO_LOWDRIVE | GPIO_OUTPUT_ONE); + + if (drdy_gpio.port != GPIO::PortInvalid) { + ret.drdy_gpio = getGPIOPort(drdy_gpio.port) | getGPIOPin(drdy_gpio.pin) | (GPIO_PULLUP | PIN_INT_BOTH); + } + + if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external) + ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid); + + } else { // it's a NuttX device (e.g. SPIDEV_FLASH(0)) + ret.devid = devid; + } + + ret.devtype_driver = PX4_SPI_DEV_ID(devid); + return ret; +} + +static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices, + GPIO::GPIOPin power_enable = {}) +{ + px4_spi_bus_t ret{}; + ret.requires_locking = false; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + ret.devices[i] = devices.devices[i]; + + + if (ret.devices[i].cs_gpio != 0) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + int same_devices_count = 0; + + for (int j = 0; j < i; ++j) { + if (ret.devices[j].cs_gpio != 0) { + same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff); + } + } + + // increment the 2. LSB byte to allow multiple devices of the same type + ret.devices[i].devid |= same_devices_count << 8; + + } else { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) + ret.requires_locking = true; + } + } + } + + ret.bus = (int)bus; + ret.is_external = false; + + if (power_enable.port != GPIO::PortInvalid) { + ret.power_enable_gpio = getGPIOPort(power_enable.port) | getGPIOPin(power_enable.pin) | + (GPIO_HIGHDRIVE | GPIO_OUTPUT_ONE); + } + + return ret; +} + +// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments +struct bus_device_external_cfg_array_t { + SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES]; +}; + +static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices) +{ + px4_spi_bus_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (devices.devices[i].cs_gpio.port == GPIO::PortInvalid) { + break; + } + + ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio); + } + + ret.bus = (int)bus; + ret.is_external = true; + ret.requires_locking = false; // external buses are never accessed by NuttX drivers + return ret; +} + +static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + SPI::bus_device_external_cfg_t ret{}; + ret.cs_gpio = cs_gpio; + ret.drdy_gpio = drdy_gpio; + return ret; +} diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c index c5bf9e29b340..9bad8959ae14 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/input_capture.c @@ -104,11 +104,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, { channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = _REG32(timer, S32K1XX_FTM_CNSC_OFFSET(chan->timer_channel - 1)) & FTM_CNSC_CHF; @@ -169,10 +169,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } else { - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - input_capture_bind(channel, callback, context); rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c index 1dffff74cbee..ec3cdf15cd9d 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c @@ -150,12 +150,12 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define CnSC_PWMIN_INIT 0 // TBD -// NotUsed PWMOut PWMIn Capture OneShot Trigger -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ -static io_timer_allocation_t once = 0; +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; typedef struct channel_stat_t { uint32_t isr_cout; @@ -267,25 +267,34 @@ static inline int validate_timer_index(unsigned timer) return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; } -static inline int is_timer_uninitalized(unsigned timer) +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) { - int rv = 0; + int ret = -EINVAL; - if (once & 1 << timer) { - rv = -EBUSY; + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; + + } else { + ret = -EBUSY; + } } - return rv; + return ret; } -static inline void set_timer_initalized(unsigned timer) +int io_timer_unallocate_timer(unsigned timer) { - once |= 1 << timer; -} + int ret = -EINVAL; -static inline void set_timer_deinitalized(unsigned timer) -{ - once &= ~(1 << timer); + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; } static inline int channels_timer(unsigned channel) @@ -322,24 +331,6 @@ static uint32_t get_timer_channels(unsigned timer) return channels_cache[timer]; } -static inline int is_channels_timer_uninitalized(unsigned channel) -{ - return is_timer_uninitalized(channels_timer(channel)); -} - -int io_timer_is_channel_free(unsigned channel) -{ - int rv = io_timer_validate_channel_index(channel); - - if (rv == 0) { - if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { - rv = -EBUSY; - } - } - - return rv; -} - int io_timer_validate_channel_index(unsigned channel) { int rv = -EINVAL; @@ -378,7 +369,6 @@ uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) return timer_io_channels[channel].gpio_in; } - int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { @@ -427,21 +417,26 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode return before ^ channels; } -static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { - int rv = io_timer_is_channel_free(channel); + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; - if (rv == 0) { + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; + ret = 0; } - return rv; + px4_leave_critical_section(flags); + + return ret; } -static inline int free_channel_resource(unsigned channel) +int io_timer_unallocate_channel(unsigned channel) { int mode = io_timer_get_channel_mode(channel); @@ -454,24 +449,6 @@ static inline int free_channel_resource(unsigned channel) return mode; } -int io_timer_free_channel(unsigned channel) -{ - if (io_timer_validate_channel_index(channel) != 0) { - return -EINVAL; - } - - int mode = io_timer_get_channel_mode(channel); - - if (mode > IOTimerChanMode_NotUsed) { - io_timer_set_enable(false, mode, 1 << channel); - free_channel_resource(channel); - - } - - return 0; -} - - static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { int rv = -EINVAL; @@ -480,7 +457,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) rv = io_timer_validate_channel_index(channel); if (rv == 0) { - rv = allocate_channel_resource(channel, mode); + rv = io_timer_allocate_channel(channel, mode); } } @@ -537,9 +514,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) px4_leave_critical_section(flags); } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channel_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channel_mask; uint32_t action_cache[MAX_IO_TIMERS] = {0}; int actions = 0; @@ -566,18 +543,20 @@ void io_timer_trigger(void) px4_leave_critical_section(flags); } -int io_timer_init_timer(unsigned timer) +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) { - /* Do this only once per timer */ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } - int rv = is_timer_uninitalized(timer); + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); - if (rv == 0) { + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { irqstate_t flags = px4_enter_critical_section(); - set_timer_initalized(timer); - /* enable the timer clock before we try to talk to it */ uint32_t regval = _REG(io_timers[timer].clock_register); @@ -656,62 +635,54 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned timer, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the timer */ - - uint32_t channels = get_timer_channels(timer); - - /* Check that all channels are either in PWM or Oneshot */ + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ - /* Change only a timer that is owned by pwm or one shot */ + uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); - /* Request to use OneShot ?*/ + /* Request to use OneShot ?*/ - if (rate == 0) { + if (PWM_RATE_ONESHOT == rate) { - /* Request to use OneShot - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be Oneshot - */ - - /* Did the allocation change */ - if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { - io_timer_set_oneshot_mode(timer); - } + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - } else { + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } - /* Request to use PWM - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be PWM - */ + } else { - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { - io_timer_set_PWM_mode(timer); - } + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - timer_set_rate(timer, rate); + if (changed_channels) { + io_timer_set_PWM_mode(timer); } - rv = OK; + timer_set_rate(timer, rate); } - return rv; + return OK; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + uint32_t gpio = timer_io_channels[channel].gpio_in; uint32_t clearbits = CnSC_RESET; uint32_t setbits = CnSC_CAPTURE_INIT; @@ -739,26 +710,32 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, return -EINVAL; } + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config + + int previous_mode = io_timer_get_channel_mode(channel); int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); - /* Valid channel should now be reserved in new mode */ + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ - if (rv >= 0) { + rv = io_timer_init_timer(timer, mode); - /* Blindly try to initialize the timer - it will only do it once */ + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } - io_timer_init_timer(channels_timer(channel)); + /* Valid channel should now be reserved in new mode */ - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { px4_arch_configgpio(gpio); } - - unsigned timer = channels_timer(channel); - /* configure the channel */ uint32_t chan = timer_io_channels[channel].timer_channel - 1; @@ -773,9 +750,10 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; - px4_leave_critical_section(flags); } + px4_leave_critical_section(flags); + return rv; } diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c index 1f5a2c6678d3..9af9b4c8e82e 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_servo.c @@ -72,40 +72,50 @@ servo_position_t up_pwm_servo_get(unsigned channel) int up_pwm_servo_init(uint32_t channel_mask) { /* Init channels */ - uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot); // First free the current set of PWMs for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (current & (1 << channel)) { - io_timer_free_channel(channel); + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); current &= ~(1 << channel); } } - // Now allocate the new set + + /* Now allocate the new set */ + + int ret_val = OK; + int channels_init_mask = 0; for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (channel_mask & (1 << channel)) { - // First free any that were not PWM mode before - - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + /* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */ - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } -void up_pwm_servo_deinit(void) +void up_pwm_servo_deinit(uint32_t channel_mask) { /* disable the timers */ - up_pwm_servo_arm(false); + up_pwm_servo_arm(false, channel_mask); } int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) @@ -129,31 +139,25 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) } } - return io_timer_set_rate(group, rate); -} - -void up_pwm_update(void) -{ - io_timer_trigger(); + return io_timer_set_pwm_rate(group, rate); } -int up_pwm_servo_set_rate(unsigned rate) +void up_pwm_update(unsigned channel_mask) { - for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { - up_pwm_servo_set_rate_group_update(i, rate); - } - - return 0; + io_timer_trigger(channel_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) { - return io_timer_get_group(group); + /* only return the set of channels in the group which we own */ + return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot)) & + io_timer_get_group(group); } void -up_pwm_servo_arm(bool armed) +up_pwm_servo_arm(bool armed, uint32_t channel_mask) { - io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS); - io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); } diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c index f2f002aa7527..5386919d388f 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/pwm_trigger.c @@ -64,23 +64,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value) int up_pwm_trigger_init(uint32_t channel_mask) { /* Init channels */ - for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { + int ret_val = OK; + int channels_init_mask = 0; - // First free any that were not trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { - io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } /* Enable the timers */ - up_pwm_trigger_arm(true); + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_trigger_deinit() diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt index 1b1504352612..89a42ac0c27a 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/CMakeLists.txt @@ -34,3 +34,5 @@ px4_add_library(arch_led_pwm led_pwm.cpp ) + +target_link_libraries(arch_led_pwm PRIVATE arch_io_pins) # io_timer_init_timer diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp index 8a7e69a526c3..f0a36da96fe1 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp @@ -306,7 +306,7 @@ led_pwm_servo_init(void) for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { #if defined(BOARD_HAS_SHARED_PWM_TIMERS) // Use the io_timer init to mark it initialized - io_timer_init_timer(i); + io_timer_init_timer(i, IOTimerChanMode_LED); #endif led_pwm_timer_init(i); } diff --git a/platforms/nuttx/src/px4/rpi/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/CMakeLists.txt new file mode 100644 index 000000000000..9d3491e36e76 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt new file mode 100644 index 000000000000..a6f9e1497e8f --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/CMakeLists.txt @@ -0,0 +1,7 @@ +add_subdirectory(../rpi_common/adc adc) +add_subdirectory(../rpi_common/hrt hrt) +add_subdirectory(../rpi_common/version version) +add_subdirectory(../rpi_common/board_reset board_reset) +add_subdirectory(../rpi_common/board_critmon board_critmon) +add_subdirectory(../rpi_common/spi spi) +add_subdirectory(../rpi_common/io_pins io_pins) diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h new file mode 100644 index 000000000000..234971efbdcd --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.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 + +#include "../../../rpi_common/include/px4_arch/adc.h" + diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.h new file mode 100644 index 000000000000..1262b2407c5a --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/hw_description.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 + + +#include "../../../rpi_common/include/px4_arch/hw_description.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..9655c49558b7 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.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 + +#include "../../../rpi_common/include/px4_arch/i2c_hw_description.h" + diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..e08db25452bb --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer.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 + + +#include "../../../rpi_common/include/px4_arch/io_timer.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 000000000000..8065d43bfa05 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/io_timer_hw_description.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 + + +#include "../../../rpi_common/include/px4_arch/io_timer_hw_description.h" diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..bb3899370345 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/micro_hal.h @@ -0,0 +1,12 @@ +#pragma once + +#include "../../../rpi_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_UNUSED +#define PX4_FLASH_BASE RP2040_FLASH_BASE +#define PX4_NUMBER_I2C_BUSES 2 +#define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 4 + +__END_DECLS diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..4c620077157c --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/spi_hw_description.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 + +#include "../../../rpi_common/include/px4_arch/spi_hw_description.h" + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { +#ifdef CONFIG_RP2040_SPI0 + true, +#else + false, +#endif +#ifdef CONFIG_RP2040_SPI1 + true, +#else + false, +#endif + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_RP2040_SPIx)"); + } + + return false; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt new file mode 100644 index 000000000000..8f0546f5ee0c --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/adc/CMakeLists.txt @@ -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. +# +############################################################################ + +px4_add_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp new file mode 100644 index 000000000000..d068fea8eb36 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/adc/adc.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 +#include +#include + +// #include Nuttx doesn't have this file in arch yet. +#include + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(base, _reg) (*(volatile uint32_t *)((base) + (_reg))) + +#define rCS(base) REG((base), 0x00) // ADC Control and Status +#define rRESULT(base) REG((base), 0x04) // Result of most recent ADC conversion +#define rFCS(base) REG((base), 0x08) // FIFO control and status +#define rFIFO(base) REG((base), 0x0c) // Conversion result FIFO +#define rDIV(base) REG((base), 0x10) // Clock divider +#define rINTR(base) REG((base), 0x14) // Raw Interrupts +#define rINTE(base) REG((base), 0x18) // Interrupt Enable +#define rINTF(base) REG((base), 0x1c) // Interrupt Force +#define rINTS(base) REG((base), 0x20) // Interrupt status after masking & forcing + +int px4_arch_adc_init(uint32_t base_address) +{ + /* Perform ADC init once per ADC */ + + static uint32_t once[SYSTEM_ADC_COUNT] {}; + + uint32_t *free = nullptr; + + for (uint32_t i = 0; i < SYSTEM_ADC_COUNT; i++) { + if (once[i] == base_address) { + + /* This one was done already */ + + return OK; + } + + /* Use first free slot */ + + if (free == nullptr && once[i] == 0) { + free = &once[i]; + } + } + + if (free == nullptr) { + + /* ADC misconfigured SYSTEM_ADC_COUNT too small */; + + PANIC(); + } + + *free = base_address; + + // Assuming that the ADC gpio is configured correctly, + // all that is left to do is divide 48MHz clock if + // necessary and then enable the ADC. (One reading + // requires about 100 clocks.) Also enable the temp + // sensor channel. + + // Divide incoming 48MHz clock + // (Trigger adc once per n+1 cycles) + // So, n >= 96. Because, 1 sample = 96 clocks + rDIV(base_address) = 0 | (0 << 8); // 8-bit fraction value | 16-bit int value => n = int + frac/256 + + // Enable temperature sensor and enable ADC + rCS(base_address) = 1 | (1 << 1); + px4_usleep(10); + + // Select temperature channel and kick off a sample and wait for it to complete + rCS(base_address) &= ~(0b111 << 12); // Clear AINSEL + rCS(base_address) |= PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL << 12; // Choose temperature channel + hrt_abstime now = hrt_absolute_time(); + rCS(base_address) |= 1 << 2; // Start a single conversion + + while (!(rCS(base_address) & (1 << 8))) { // Check if the sample is ready + + /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 500) { + return -1; + } + } + + /* Read out result */ + (void) rRESULT(base_address); + + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + // Disable ADC + rCS(base_address) = 0; +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* run a single conversion right now - should take about 96 cycles (a few microseconds) max */ + rCS(base_address) &= ~(0b111 << 12); // Clear AINSEL + rCS(base_address) |= channel << 12; // Choose temperature channel + rCS(base_address) |= 1 << 2; // Start a single conversion + + /* wait for the conversion to complete */ + const hrt_abstime now = hrt_absolute_time(); + + while (!(rCS(base_address) & (1 << 8))) { // Check if the sample is ready + + /* don't wait for more than 50us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 50) { + px4_leave_critical_section(flags); + return UINT32_MAX; + } + } + + /* read the result and clear EOC */ + uint32_t result = rRESULT(base_address); + + px4_leave_critical_section(flags); + + return result; +} + +float px4_arch_adc_reference_v() +{ + return BOARD_ADC_POS_REF_V; // TODO: provide true vref +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + + return 1 << PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL; + +} + +uint32_t px4_arch_adc_dn_fullcount() +{ + return 1 << 12; // 12 bit ADC +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt new file mode 100644 index 000000000000..3edcbee06203 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/CMakeLists.txt @@ -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. +# +############################################################################ + +px4_add_library(arch_board_critmon + board_critmon.c +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c new file mode 100644 index 000000000000..14e7a3bb3bb4 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_critmon/board_critmon.c @@ -0,0 +1,66 @@ +/************************************************************************************ + * + * Copyright (C) 2018 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 + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "missing implementation for up_critmon_gettime() and up_critmon_convert()" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +// uint32_t up_critmon_gettime(void) +// { +// } + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +// void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +// { +// } + +#endif /* CONFIG_SCHED_CRITMONITOR */ diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt new file mode 100644 index 000000000000..d1fb1802c308 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_board_reset + board_reset.cpp +) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp new file mode 100644 index 000000000000..058f07aea118 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/board_reset/board_reset.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * Author: @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. + * + ****************************************************************************/ + +/** + * @file board_reset.cpp + * Implementation of RP2040 based Board RESET API + */ + +#include +#include +#include + +#include + +// Functions in here are modified so that board_reset() function resembles +// the one available in nuttx's boards/raspberrypi-pico folder. + +#ifdef CONFIG_BOARDCTL_RESET + +/**************************************************************************** + * Name: board_configure_reset + * + * Description: + * Configures the device that maintains the state shared by the + * application and boot loader. This is usually an RTC. + * + * Input Parameters: + * mode - The type of reset. See reset_mode_e + * + * Returned Value: + * 0 for Success + * 1 if invalid argument + * + ****************************************************************************/ + +// static const uint32_t modes[] = { +// /* to tb */ +// /* BOARD_RESET_MODE_CLEAR 5 y */ 0, +// /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb007b007, +// /* BOARD_RESET_MODE_BOOT_TO_VALID_APP 0 y */ 0xb0070002, +// /* BOARD_RESET_MODE_CAN_BL 10 n */ 0xb0080000, +// /* BOARD_RESET_MODE_RTC_BOOT_FWOK 0 n */ 0xb0093a26 +// }; + +// int board_configure_reset(reset_mode_e mode, uint32_t arg) +// { +// int rv = -1; + +// if (mode < arraySize(modes)) { + +// stm32_pwr_enablebkp(true); + +// arg = mode == BOARD_RESET_MODE_CAN_BL ? arg & ~0xff : 0; + +// // Check if we can to use the new register definition + +// #ifndef STM32_RTC_BK0R +// *(uint32_t *)STM32_BKP_BASE = modes[mode] | arg; +// #else +// *(uint32_t *)STM32_RTC_BK0R = modes[mode] | arg; +// #endif +// stm32_pwr_enablebkp(false); +// rv = OK; +// } + +// return rv; +// } + +/**************************************************************************** + * Name: board_reset + * + * Description: + * Reset board. Support for this function is required by board-level + * logic if CONFIG_BOARDCTL_RESET is selected. + * + * Input Parameters: + * status - Status information provided with the reset event. This + * meaning of this status information is board-specific. If not + * used by a board, the value zero may be provided in calls to + * board_reset(). + * + * Returned Value: + * If this function returns, then it was not possible to power-off the + * board due to some constraints. The return value int this case is a + * board-specific reason for the failure to shutdown. + * + ****************************************************************************/ + +int board_reset(int status) +{ + if (status == 1) { + // board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); + } + +#if defined(BOARD_HAS_ON_RESET) + // board_on_reset(status); +#endif + + up_systemreset(); + return 0; +} + +#endif /* CONFIG_BOARDCTL_RESET */ + +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) +/**************************************************************************** + * Name: board_booted_by_px4 + * + * Description: + * Determines if the the boot loader was PX4 + * + * Input Parameters: + * none + * + * Returned Value: + * true if booted byt a PX4 bootloader. + * + ****************************************************************************/ +bool board_booted_by_px4(void) +{ + uint32_t *vectors = (uint32_t *) STM32_FLASH_BASE; + + /* Nuttx uses common vector */ + + return (vectors[2] == vectors[3]) && (vectors[4] == vectors[5]); +} +#endif diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt new file mode 100644 index 000000000000..9ce26ff632d3 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_hrt + hrt.c +) +target_compile_options(arch_hrt + PRIVATE + ${MAX_CUSTOM_OPT_LEVEL} + -Wno-cast-align # TODO: fix and enable +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c new file mode 100644 index 000000000000..12050a247d38 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/hrt/hrt.c @@ -0,0 +1,772 @@ +/**************************************************************************** + * + * 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 drv_hrt.c + * + * High-resolution timer callouts and timekeeping. + * + * RP2040's internal 64 bit timer can be used for this purpose. + * + * Note that really, this could use systick too, but that's + * monopolised by NuttX and stealing it would just be awkward. + * + * We don't use the NuttX RP2040 driver per se; rather, we + * claim the timer and then drive it directly. + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + + +// #include "rp2040_gpio.h" + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +// RP2040 has a dedicated 64-bit timer which updates at 1MHz. This timer can be used here as hrt. +// The advantage is that this timer will not overflow as it can run for thousands of years. +// This timer is activated when the clk_ref is configured for watchdog and TICK is enabled. +// Fortunately, nuttx by default does this for us in rp2040_clock.c file. Thus, no init required. +// Four individual interrupts can be configured which are triggered when the lower 32-bits of the timer +// matches with the value in ALARMx register. This allows for the interrupt to fire at ~72 min in future. +// Take a look at src/drivers/drv_hrt.h to find all the necessary functions required to be implemented. + +#ifdef HRT_TIMER + +/* HRT configuration */ +#if HRT_TIMER == 1 +# define HRT_TIMER_BASE RP2040_TIMER_BASE +#else +# error HRT_TIMER must have a value of 1 because there is only one timer in RP2040 +#endif + +/** + * Minimum/maximum deadlines. + * + * These are suitable for use with a 16-bit timer/counter clocked + * at 1MHz. The high-resolution timer need only guarantee that it + * not wrap more than once in the 50ms period for absolute time to + * be consistently maintained. + * + * The minimum deadline must be such that the time taken between + * reading a time and writing a deadline to the timer cannot + * result in missing the deadline. + */ +#define HRT_INTERVAL_MIN 50 +#define HRT_INTERVAL_MAX 4000000000 + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ +#define HRT_COUNTER_SCALE(_c) (_c) + +/* + * Timer register accessors + */ +#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg)) + +#define rTIMEHW REG(0x0) // Write to bits 63:32 of time always write timelw before timehw +#define rTIMELW REG(0x4) // Write to bits 31:0 of time writes do not get copied to time until timehw is written +#define rTIMEHR REG(0x8) // Read from bits 63:32 of time always read timelr before timehr +#define rTIMELR REG(0xc) // Read from bits 31:0 of time +#define rALARM0 REG(0x10) // Arm alarm 0, and configure the time it will fire +#define rALARM1 REG(0x14) // Arm alarm 1, and configure the time it will fire +#define rALARM2 REG(0x18) // Arm alarm 2, and configure the time it will fire +#define rALARM3 REG(0x1c) // Arm alarm 3, and configure the time it will fire +#define rARMED REG(0x20) // Indicates the armed/disarmed status of each alarm +#define rTIMERAWH REG(0x24) // Raw read from bits 63:32 of time +#define rTIMERAWL REG(0x28) // Raw read from bits 31:0 of time +#define rDBGPAUSE REG(0x2c) // Set bits high to enable pause when the corresponding debug ports are active +#define rPAUSE REG(0x30) // Set high to pause the timer +#define rINTR REG(0x34) // Raw Interrupts +#define rINTE REG(0x38) // Interrupt Enable +#define rINTF REG(0x3c) // Interrupt Force +#define rINTS REG(0x40) // Interrupt status after masking & forcing + +/* + * Specific registers and bits used by HRT sub-functions + */ +#if HRT_TIMER_CHANNEL == 1 +# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_0 // Timer alarm interrupt vector // +# define HRT_ALARM_VALUE rALARM0 // Alarm register for HRT (similar to compare register for other MCUs) // +# define HRT_ALARM_ENABLE (1 << 0) // Bit-0 for alarm 0 // +#elif HRT_TIMER_CHANNEL == 2 +# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_1 // Timer alarm interrupt vector // +# define HRT_ALARM_VALUE rALARM1 // Alarm register for HRT (similar to compare register for other MCUs) // +# define HRT_ALARM_ENABLE (1 << 1) // Bit-1 for alarm 1 // +#elif HRT_TIMER_CHANNEL == 3 +# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_2 // Timer alarm interrupt vector // +# define HRT_ALARM_VALUE rALARM2 // Alarm register for HRT (similar to compare register for other MCUs) // +# define HRT_ALARM_ENABLE (1 << 2) // Bit-2 for alarm 2 // +#elif HRT_TIMER_CHANNEL == 4 +# define HRT_TIMER_VECTOR RP2040_TIMER_IRQ_3 // Timer alarm interrupt vector // +# define HRT_ALARM_VALUE rALARM3 // Alarm register for HRT (similar to compare register for other MCUs) // +# define HRT_ALARM_ENABLE (1 << 3) // Bit-3 for alarm 3 // +#else +# error HRT_TIMER_CHANNEL must be a value between 1 and 4 +#endif + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* latency baseline (last compare value applied) */ +static uint64_t latency_baseline; + +/* timer count at interrupt (for latency purposes) */ +static uint64_t latency_actual; + +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context, void *arg); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, + hrt_abstime deadline, + hrt_abstime interval, + hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +// For PPM, a PWM timer is not required to be used. This will be handled by activating +// edge detection interrupts on the gpio. This way, the value of hrt can be recorded +// when irq_handler is called and the PPM signal can be decoded. Also, there is no way +// to check wheter the interrupt was caused by a rising edge or a falling edge. This can +// be done by changing the interrupt type (rising/falling) every time the interrupt is +// triggered. + +/* + * Specific registers and bits used by PPM sub-functions + */ +#ifdef HRT_PPM_CHANNEL +/* + * PPM decoder tuning parameters + */ +# define PPM_MIN_PULSE_WIDTH 200 /**< minimum width of a valid first pulse */ +# define PPM_MAX_PULSE_WIDTH 600 /**< maximum width of a valid first pulse */ +# define PPM_MIN_CHANNEL_VALUE 800 /**< shortest valid channel signal */ +# define PPM_MAX_CHANNEL_VALUE 2200 /**< longest valid channel signal */ +# define PPM_MIN_START 2300 /**< shortest valid start gap (only 2nd part of pulse) */ + +/* decoded PPM buffer */ +#define PPM_MIN_CHANNELS 5 +#define PPM_MAX_CHANNELS 20 + +/** Number of same-sized frames required to 'lock' */ +#define PPM_CHANNEL_LOCK 4 /**< should be less than the input timeout */ + +__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; +__EXPORT unsigned ppm_decoded_channels = 0; +__EXPORT uint64_t ppm_last_valid_decode = 0; + +#define PPM_DEBUG 0 + +#if PPM_DEBUG +/* PPM edge history */ +__EXPORT uint16_t ppm_edge_history[32]; +unsigned ppm_edge_next; + +/* PPM pulse history */ +__EXPORT uint16_t ppm_pulse_history[32]; +unsigned ppm_pulse_next; +#endif + +static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; + +/** PPM decoder state machine */ +struct { + uint16_t last_edge; /**< last capture time */ + uint16_t last_mark; /**< last significant edge */ + uint16_t frame_start; /**< the frame width */ + unsigned next_channel; /**< next channel index */ + enum { + UNSYNCH = 0, + ARM, + ACTIVE, + INACTIVE + } phase; +} ppm; + +// Store the last detected edge type +enum edgeType { + falling = 0, + rising, +}; +enum edgeType lastEdge = falling; + +// PPM specific functions +static void hrt_ppm_decode(uint16_t counterVal); +static int hrt_ppm_isr(int irq, void *context, void *arg); + +#endif /* HRT_PPM_CHANNEL */ + +/** + * Initialise the timer we are going to use. + */ +static void +hrt_tim_init(void) +{ + /* claim our interrupt vector */ + irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL); + + /* Enable alarm interrupt */ + rINTE = HRT_ALARM_ENABLE; + + /* Enable vector interrupt */ + up_enable_irq(HRT_TIMER_VECTOR); + + /* Set Initial capture a little ways off */ + HRT_ALARM_VALUE = rTIMERAWL + 1000; +} + +#ifdef HRT_PPM_CHANNEL +/** + * Handle the PPM decoder state machine. + */ +static void +hrt_ppm_decode(uint16_t counterVal) +{ + uint16_t count = counterVal; + uint16_t width; + uint16_t interval; + unsigned i; + + // If we miss an edge then we will miss a whole pulse. + // So, frame discarding will be taken care of by the + // width value. + + /* how long since the last edge? - this handles counter wrapping implicitly. */ + width = count - ppm.last_edge; + +#if PPM_DEBUG + ppm_edge_history[ppm_edge_next++] = width; + + if (ppm_edge_next >= 32) { + ppm_edge_next = 0; + } + +#endif + + /* + * if this looks like a start pulse, then push the last set of values + * and reset the state machine + */ + if (width >= PPM_MIN_START) { + + /* + * If the number of channels changes unexpectedly, we don't want + * to just immediately jump on the new count as it may be a result + * of noise or dropped edges. Instead, take a few frames to settle. + */ + if (ppm.next_channel != ppm_decoded_channels) { + static unsigned new_channel_count; + static unsigned new_channel_holdoff; + + if (new_channel_count != ppm.next_channel) { + /* start the lock counter for the new channel count */ + new_channel_count = ppm.next_channel; + new_channel_holdoff = PPM_CHANNEL_LOCK; + + } else if (new_channel_holdoff > 0) { + /* this frame matched the last one, decrement the lock counter */ + new_channel_holdoff--; + + } else { + /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ + ppm_decoded_channels = new_channel_count; + new_channel_count = 0; + } + + } else { + /* frame channel count matches expected, let's use it */ + if (ppm.next_channel >= PPM_MIN_CHANNELS) { + for (i = 0; i < ppm.next_channel; i++) { + ppm_buffer[i] = ppm_temp_buffer[i]; + } + + ppm_last_valid_decode = hrt_absolute_time(); + + } + } + + /* reset for the next frame */ + ppm.next_channel = 0; + + /* next edge is the reference for the first channel */ + ppm.phase = ARM; + + ppm.last_edge = count; + return; + } + + switch (ppm.phase) { + case UNSYNCH: + /* we are waiting for a start pulse - nothing useful to do here */ + break; + + case ARM: + + /* we expect a pulse giving us the first mark */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* record the mark timing, expect an inactive edge */ + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; + + case INACTIVE: + + /* we expect a short pulse */ + if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH) { + goto error; /* pulse was too short or too long */ + } + + /* this edge is not interesting, but now we are ready for the next mark */ + ppm.phase = ACTIVE; + break; + + case ACTIVE: + /* determine the interval from the last mark */ + interval = count - ppm.last_mark; + ppm.last_mark = count; + +#if PPM_DEBUG + ppm_pulse_history[ppm_pulse_next++] = interval; + + if (ppm_pulse_next >= 32) { + ppm_pulse_next = 0; + } + +#endif + + /* if the mark-mark timing is out of bounds, abandon the frame */ + if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) { + goto error; + } + + /* if we have room to store the value, do so */ + if (ppm.next_channel < PPM_MAX_CHANNELS) { + ppm_temp_buffer[ppm.next_channel++] = interval; + } + + ppm.phase = INACTIVE; + break; + + } + + ppm.last_edge = count; + return; + + /* the state machine is corrupted; reset it */ + +error: + /* we don't like the state of the decoder, reset it and try again */ + ppm.phase = UNSYNCH; + ppm_decoded_channels = 0; + +} +#endif /* HRT_PPM_CHANNEL */ + +/** + * Handle the compare interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context, void *arg) +{ + /* grab the timer for latency tracking purposes */ + latency_actual = hrt_absolute_time(); + + rINTR = rINTR; // ack the interrupts we just read + + /* It is never a timer tick. It is always triggered by alarm. */ + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + return OK; +} + +/** + * Handle the compare interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_ppm_isr(int irq, void *context, void *arg) +{ + // Read lower 16 bits of hrt + uint16_t counter = rTIMERAWL & 0xffff; + + // Switch the next interrupt type + px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL); + lastEdge = !lastEdge; + + hrt_ppm_decode(counter); + return OK; +} + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime hrt_absolute_time(void) +{ + /* Taken from rp2040 datasheet pg. 558 */ + uint32_t hi = rTIMERAWH; + uint32_t lo; + + do { + lo = rTIMERAWL; + uint32_t next_hi = rTIMERAWH; + + if (hi == next_hi) { break; } + + hi = next_hi; + } while (true); + + return ((uint64_t) hi << 32) | lo; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +void hrt_store_absolute_time(volatile hrt_abstime *t) +{ + irqstate_t flags = px4_enter_critical_section(); + *t = hrt_absolute_time(); + px4_leave_critical_section(flags); +} + +/** + * Initialise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); + +#ifdef HRT_PPM_CHANNEL + // Set up edge detection interrupt on the PPM gpio. + px4_arch_gpiosetevent(GPIO_PPM_IN, lastEdge ? false : true, lastEdge ? true : false, true, hrt_ppm_isr, NULL); + lastEdge = !lastEdge; + + /* configure the PPM input pin */ + px4_arch_configgpio(GPIO_PPM_IN); +#endif +} + +/** + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/** + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/** + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + px4_leave_critical_section(flags); +} + +/** + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/** + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = px4_enter_critical_section(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + px4_leave_critical_section(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrtinfo("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + hrtinfo("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + hrt_abstime deadline = now + HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + * Note that we ensure that this will be within the counter + * period, so that when we truncate all but the low 16 bits + * the next time the compare matches it will be the deadline + * we want. + * + * It is important for accurate timekeeping that the compare + * interrupt fires sufficiently often that the base_time update in + * hrt_absolute_time runs at least once per timer period. + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = now + HRT_INTERVAL_MIN; + + } else if (next->deadline < deadline) { + hrtinfo("due soon\n"); + deadline = next->deadline; + } + } + + hrtinfo("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); + + /* set the new compare value and remember it for latency tracking */ + HRT_ALARM_VALUE = (latency_baseline = deadline) & 0xffffffff; +} + +static void +hrt_latency_update(void) +{ + uint16_t latency = latency_actual - latency_baseline; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} + +#endif /* HRT_TIMER */ diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h new file mode 100644 index 000000000000..96aa40bc333b --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +#include + +#define SYSTEM_ADC_BASE RP2040_ADC_BASE + +#include + diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h new file mode 100644 index 000000000000..5588a45c70c7 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/hw_description.h @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * 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 + +#include + +#include + +#include + + +/* + * Timers + */ + +namespace Timer +{ +enum Timer { + Timer0 = 1, + Timer1, + Timer2, + Timer3, + Timer4, + Timer5, + Timer6, + Timer7, +}; +enum Channel { + ChannelA = 1, + ChannelB, +}; +struct TimerChannel { + Timer timer; + Channel channel; +}; +} + +static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer) +{ + switch (timer) { + case Timer::Timer0: return RP2040_PWM_BASE + 0x00; + + case Timer::Timer1: return RP2040_PWM_BASE + 0x14; + + case Timer::Timer2: return RP2040_PWM_BASE + 0x28; + + case Timer::Timer3: return RP2040_PWM_BASE + 0x3c; + + case Timer::Timer4: return RP2040_PWM_BASE + 0x50; + + case Timer::Timer5: return RP2040_PWM_BASE + 0x64; + + case Timer::Timer6: return RP2040_PWM_BASE + 0x78; + + case Timer::Timer7: return RP2040_PWM_BASE + 0x8c; + + default: break; + } + + return 0; +} + + +/* + * GPIO + */ + +namespace GPIO +{ +// RP2040 doesn't have PORTS +enum Pin { + Pin0 = 0, + Pin1, + Pin2, + Pin3, + Pin4, + Pin5, + Pin6, + Pin7, + Pin8, + Pin9, + Pin10, + Pin11, + Pin12, + Pin13, + Pin14, + Pin15, + Pin16, + Pin17, + Pin18, + Pin19, + Pin20, + Pin21, + Pin22, + Pin23, + Pin24, + Pin25, + Pin26, + Pin27, + Pin28, + Pin29, +}; + +struct GPIOPin { + Pin pin; +}; +} + +static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin) +{ + switch (pin) { + case GPIO::Pin0: return 0; + + case GPIO::Pin1: return 1; + + case GPIO::Pin2: return 2; + + case GPIO::Pin3: return 3; + + case GPIO::Pin4: return 4; + + case GPIO::Pin5: return 5; + + case GPIO::Pin6: return 6; + + case GPIO::Pin7: return 7; + + case GPIO::Pin8: return 8; + + case GPIO::Pin9: return 9; + + case GPIO::Pin10: return 10; + + case GPIO::Pin11: return 11; + + case GPIO::Pin12: return 12; + + case GPIO::Pin13: return 13; + + case GPIO::Pin14: return 14; + + case GPIO::Pin15: return 15; + + case GPIO::Pin16: return 16; + + case GPIO::Pin17: return 17; + + case GPIO::Pin18: return 18; + + case GPIO::Pin19: return 19; + + case GPIO::Pin20: return 20; + + case GPIO::Pin21: return 21; + + case GPIO::Pin22: return 22; + + case GPIO::Pin23: return 23; + + case GPIO::Pin24: return 24; + + case GPIO::Pin25: return 25; + + case GPIO::Pin26: return 26; + + case GPIO::Pin27: return 27; + + case GPIO::Pin28: return 28; + + case GPIO::Pin29: return 29; + } + + return 0; +} + +namespace SPI +{ +enum class Bus { + SPI0 = 1, + SPI1, +}; + +using CS = GPIO::GPIOPin; ///< chip-select pin +using DRDY = GPIO::GPIOPin; ///< data ready pin + +struct bus_device_external_cfg_t { + CS cs_gpio; + DRDY drdy_gpio; +}; + +} // namespace SPI diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..5254f9effa65 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 + +#include +#include + + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} 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 new file mode 100644 index 000000000000..c9fbedd8f0bc --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer.h @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * 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 io_timer.h + */ +#include +#include +#include + +#include + +#pragma once +__BEGIN_DECLS +/* configuration limits */ +#ifdef BOARD_NUM_IO_TIMERS +#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS +#else +#define MAX_IO_TIMERS 8 +#endif +#if DIRECT_PWM_OUTPUT_CHANNELS > 8 +#define MAX_TIMER_IO_CHANNELS DIRECT_PWM_OUTPUT_CHANNELS +#else +#define MAX_TIMER_IO_CHANNELS 16 +#endif + +#define MAX_LED_TIMERS 2 +#define MAX_TIMER_LED_CHANNELS 4 + +#define IO_TIMER_ALL_MODES_CHANNELS 0 + +/* TIM_DMA_Base_address TIM DMA Base Address */ +#define TIM_DMABASE_CCR1 0x0000000DU +#define TIM_DMABASE_CCR2 0x0000000EU +#define TIM_DMABASE_CCR3 0x0000000FU +#define TIM_DMABASE_CCR4 0x00000010U + +typedef enum io_timer_channel_mode_t { + IOTimerChanMode_NotUsed = 0, + IOTimerChanMode_PWMOut = 1, + IOTimerChanMode_PWMIn = 2, + IOTimerChanMode_Capture = 3, + IOTimerChanMode_OneShot = 4, + IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, + IOTimerChanModeSize +} io_timer_channel_mode_t; + +typedef uint16_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */ + +/* array of timers dedicated to PWM in and out and capture use + *** Note that the clock_freq is set to the source in the clock tree that + *** feeds this specific timer. This can differs by Timer! + *** In PWM mode the timer's prescaler is set to achieve a counter frequency of 1MHz + *** In OneShot mode the timer's prescaler is set to achieve a counter frequency of 8MHz + *** Other prescaler rates can be achieved by fore instance by setting the clock_freq = 1Mhz + *** the resulting PSC will be one and the timer will count at it's clock frequency. + */ +typedef struct io_timers_t { + uint32_t base; + // uint32_t clock_register; // Not required for rp2040 + // uint32_t clock_bit; + // uint32_t vectorno; + // dshot_conf_t dshot; +} io_timers_t; + +typedef struct io_timers_channel_mapping_element_t { + uint32_t first_channel_index; + uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; +} io_timers_channel_mapping_element_t; + +/* mapping for each io_timers to timer_io_channels */ +typedef struct io_timers_channel_mapping_t { + io_timers_channel_mapping_element_t element[MAX_IO_TIMERS]; +} io_timers_channel_mapping_t; + +/* array of channels in logical order */ +typedef struct timer_io_channels_t { + uint32_t gpio_out; + uint32_t gpio_in; + uint8_t timer_index; + uint8_t timer_channel; +} timer_io_channels_t; + +typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint32_t chan_index, + const timer_io_channels_t *chan, + hrt_abstime isrs_time, uint16_t isrs_rcnt); + + +/* supplied by board-specific code */ +__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS]; +__EXPORT extern const io_timers_channel_mapping_t io_timers_channel_mapping; +__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS]; + +__EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; +__EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; + +__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; + +__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context); + +__EXPORT int io_timer_init_timer(unsigned timer); + +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, + io_timer_channel_allocation_t masks); +__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT uint16_t io_channel_get_ccr(unsigned channel); +__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); +__EXPORT uint32_t io_timer_get_group(unsigned timer); +__EXPORT int io_timer_validate_channel_index(unsigned channel); +__EXPORT int io_timer_is_channel_free(unsigned channel); +__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_get_channel_mode(unsigned channel); +__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); +__EXPORT extern void io_timer_trigger(void); +__EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); + +__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); + +/** + * Returns the pin configuration for a specific channel, to be used as GPIO output. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); +/** + * Returns the pin configuration for a specific channel, to be used as PWM input. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); + +__END_DECLS diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h new file mode 100644 index 000000000000..e76cddff4e2d --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/io_timer_hw_description.h @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * 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 + + +#include +#include +#include +#include +#include + +static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + Timer::TimerChannel timer, GPIO::GPIOPin pin) +{ + timer_io_channels_t ret{}; + + uint32_t gpio_af = 0; + + switch (timer.channel) { + case Timer::ChannelA: + if (!(pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) { + gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM); + } + + break; + + case Timer::ChannelB: + if ((pin.pin & 1) && (pin.pin & 15) / 2 == (timer.timer - 1)) { + gpio_af = getGPIOPin(pin.pin) | GPIO_FUN(RP2040_GPIO_FUNC_PWM); + } + + break; + + default: + break; + } + + ret.gpio_in = gpio_af; + ret.gpio_out = gpio_af; + + ret.timer_channel = timer.channel; + + // find timer index + ret.timer_index = 0xff; + const uint32_t timer_base = timerBaseRegister(timer.timer); + + for (int i = 0; i < MAX_IO_TIMERS; ++i) { + if (io_timers_conf[i].base == timer_base) { + ret.timer_index = i; + break; + } + } + + constexpr_assert(ret.timer_index != 0xff, "Timer not found"); + + return ret; +} + +static inline constexpr io_timers_t initIOTimer(Timer::Timer timer) +{ + bool nuttx_config_timer_enabled = false; + io_timers_t ret{}; + + switch (timer) { + case Timer::Timer0: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH0 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer1: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH1 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer2: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH2 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer3: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH3 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer4: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH4 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer5: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH5 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer6: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH6 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer7: + ret.base = timerBaseRegister(timer); +#ifdef CONFIG_RP2040_PWM_CH7 // Currently Nuttx doesn't have PWM support for RP2040. This is for possible future use. + nuttx_config_timer_enabled = true; +#endif + break; + } + + // This is not strictly required, but for consistency let's make sure NuttX timers are disabled + constexpr_assert(!nuttx_config_timer_enabled, "IO Timer requires NuttX timer config to be disabled (KINETIS_FTMx)"); + + return ret; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..1188b0704ea5 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/micro_hal.h @@ -0,0 +1,107 @@ +#pragma once + +#include + +__BEGIN_DECLS + +#include +#include +#include + +// RP2040 doesn't have a bbsram. Following two defines are copied from nxp/k66. +// This will remove the errors of undefined PX4_BBSRAM_SIZE when logger module is activated. +// Fixme: using ?? +#define PX4_BBSRAM_SIZE 2048 +#define PX4_BBSRAM_GETDESC_IOCTL 0 + +// RP2040 doesn't really have a cpu register with unique id. +// However, there is a function in pico-sdk which can provide +// a device unique id from its flash which is 64 bits in length. +// For now, a common device id will be used for all RP2040 based devices. +// Take a look at board_identity.c file in version folder. + +#define PX4_CPU_UUID_BYTE_LENGTH 12 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* By not defining PX4_CPU_UUID_CORRECT_CORRELATION the following maintains the legacy incorrect order + * used for selection of significant digits of the UUID in the PX4 code base. + * This is done to avoid the ripple effects changing the IDs used on STM32 base platforms + */ +#if defined(PX4_CPU_UUID_CORRECT_CORRELATION) +# define PX4_CPU_UUID_WORD32_UNIQUE_H 0 /* Least significant digits change the most */ +# define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Middle significant digits */ +# define PX4_CPU_UUID_WORD32_UNIQUE_L 2 /* Most significant digits change the least */ +#else +/* Legacy incorrect ordering */ +# define PX4_CPU_UUID_WORD32_UNIQUE_H 2 /* Most significant digits change the least */ +# define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Middle significant digits */ +# define PX4_CPU_UUID_WORD32_UNIQUE_L 0 /* Least significant digits change the most */ +#endif + +/* Separator nnn:nnn:nnnn 2 char per byte term */ +#define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) +#define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) + +#define px4_savepanic(fileno, context, length) (0) // Turn off px4_savepanic for rp2040 as it is not implemented in nuttx + +#define PX4_BUS_OFFSET 1 /* RP2040 buses are 0 based and adjustment is needed */ +#define px4_spibus_initialize(bus_num_1based) rp2040_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) rp2040_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) rp2040_i2cbus_uninitialize(pdev) + +// This part of the code is specific to rp2040. +// RP2040 does not have the gpio configuration process similar to stm or tiva devices. +// There are multiple different registers which are required to be configured based on the function selection. +// However, only five values are required for the most part: Pin number, Pull up/down, direction, set/clear and function +// The pinset below can be defined using a 16-bit value where, +// bits Function +// 0-4 GPIO number. 0-29 is valid. +// 5 Pull up +// 6 Pull down +// 7 Direction +// 8 Set/clear +// 9-13 GPIO function select +// 14-15 Unused +#define GPIO_PU (1 << 5) // Pull-up resistor +#define GPIO_PD (1 << 6) // Pull-down resistor +#define GPIO_OUT (1 << 7) // Output enable +#define GPIO_SET (1 << 8) // Output set +#define GPIO_FUN(func) (func << 9) // Function select + +#define GPIO_NUM_MASK 0x1f +#define GPIO_PU_MASK 0x20 // GPIO PAD register mask +#define GPIO_PD_MASK 0x40 // GPIO pin number mask +#define GPIO_OUT_MASK 0x80 // GPIO pin function mask +#define GPIO_SET_MASK 0x100 // GPIO pin function mask +#define GPIO_FUN_MASK 0x3E00 // GPIO output enable mask + +int rp2040_gpioconfig(uint32_t pinset); +int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg); + +#define px4_arch_configgpio(pinset) rp2040_gpioconfig(pinset) // Defined in io_pins/rp2040_pinset.c +#define px4_arch_unconfiggpio(pinset) rp2040_gpio_init(pinset & GPIO_NUM_MASK) // Reset the pin as input SIO +#define px4_arch_gpioread(pinset) rp2040_gpio_get(pinset & GPIO_NUM_MASK) // Use gpio_get +#define px4_arch_gpiowrite(pinset, value) rp2040_gpio_put(pinset & GPIO_NUM_MASK, value) // Use gpio_put +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) rp2040_setgpioevent(pinset,r,f,e,fp,a) // Defined in io_pins/rp2040_pinset.c + +// Following are quick defines to be used with the functions defined above +// These defines create a bit-mask which is supposed to be used in the +// functions defined above to set up gpios correctly. +#define PX4_MAKE_GPIO_INPUT(gpio) (gpio | GPIO_PU | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) +#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (gpio | GPIO_OUT | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) +#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (gpio | GPIO_OUT | GPIO_SET | GPIO_FUN(RP2040_GPIO_FUNC_SIO)) + +#define PX4_GPIO_PIN_OFF(pinset) ((pinset & GPIO_NUM_MASK) | GPIO_FUN(RP2040_GPIO_FUNC_SIO) | GPIO_PD) + +#define px4_cache_aligned_data() +#define px4_cache_aligned_alloc malloc + +__END_DECLS diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..4279b955b795 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/spi_hw_description.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include + +static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + px4_spi_bus_device_t ret{}; + ret.cs_gpio = getGPIOPin(cs_gpio.pin) | (GPIO_OUT | GPIO_SET); + ret.drdy_gpio = getGPIOPin(drdy_gpio.pin) | GPIO_PU; // GPIO_PU taken from kinetis + + if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external) + ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid); + + } else { // it's a NuttX device (e.g. SPIDEV_FLASH(0)) + ret.devid = devid; + } + + ret.devtype_driver = PX4_SPI_DEV_ID(devid); + return ret; +} + +static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus_devices_t &devices, + GPIO::GPIOPin power_enable = {}) +{ + px4_spi_bus_t ret{}; + ret.requires_locking = false; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + ret.devices[i] = devices.devices[i]; + + + if (ret.devices[i].cs_gpio != 0) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + int same_devices_count = 0; + + for (int j = 0; j < i; ++j) { + if (ret.devices[j].cs_gpio != 0) { + same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff); + } + } + + // increment the 2. LSB byte to allow multiple devices of the same type + ret.devices[i].devid |= same_devices_count << 8; + + } else { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) + ret.requires_locking = true; + } + } + } + + ret.bus = (int)bus; + ret.is_external = false; + + ret.power_enable_gpio = getGPIOPin(power_enable.pin) | GPIO_OUT; + + return ret; +} + +// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments +struct bus_device_external_cfg_array_t { + SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES]; +}; + +static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices) +{ + px4_spi_bus_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio); + } + + ret.bus = (int)bus; + ret.is_external = true; + ret.requires_locking = false; // external buses are never accessed by NuttX drivers + return ret; +} + +static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + SPI::bus_device_external_cfg_t ret{}; + ret.cs_gpio = cs_gpio; + ret.drdy_gpio = drdy_gpio; + return ret; +} + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt new file mode 100644 index 000000000000..889c5004ddb2 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/CMakeLists.txt @@ -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. +# +############################################################################ + +px4_add_library(arch_io_pins + io_timer.c + pwm_servo.c + # pwm_trigger.c + # input_capture.c + rp2040_pinset.c +) + +target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries(arch_io_pins PRIVATE drivers_board) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c new file mode 100644 index 000000000000..b519a8da2400 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/io_timer.c @@ -0,0 +1,768 @@ +/**************************************************************************** + * + * 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 io_timer.c + * + * Servo driver supporting PWM servos connected to RP2040 PWM blocks. + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +// RP2040 PWM block has 8 slices (timers) and each has 2 independent outputs (channels) A and B. +// All the channels can output pwm. However, only channel B can be used for input where the timer +// will be working in edge sensitive mode or level sensitive mode. Be careful when choosing the +// timer and channel for input capture. Each timer has an independent 8.4 fractional divider. +// Each timer also has double buffered wrap (rTOP) and level (rCC) registers so the value can +// change while PWM is running. + +#if !defined(BOARD_PWM_FREQ) +#define BOARD_PWM_FREQ 1000000 +#endif + +#if !defined(BOARD_ONESHOT_FREQ) +#define BOARD_ONESHOT_FREQ 8000000 +#endif + +#define TIM_SRC_CLOCK_FREQ 125000000 + +#define MAX_CHANNELS_PER_TIMER 2 + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) +#define _REG32(_base, _reg) (*(volatile uint32_t *)(_base + _reg)) +#define REG(_tmr, _reg) _REG32(io_timers[_tmr].base, _reg) + +// Register offsets +#define RP2040_PWM_CSR_OFFSET 0x00 // Control and status register +#define RP2040_PWM_DIV_OFFSET 0x04 // Clock divide register +#define RP2040_PWM_CTR_OFFSET 0x08 // Direct access to the PWM counter +#define RP2040_PWM_CCR_OFFSET 0x0c // Counter compare values +#define RP2040_PWM_TOP_OFFSET 0x10 // Counter wrap value +#define RP2040_PWM_EN_OFFSET 0xa0 // This register aliases the CSR_EN bits for all channels +#define RP2040_PWM_INTR_OFFSET 0xa4 // Raw Interrupts +#define RP2040_PWM_INTE_OFFSET 0xa8 // Interrupt Enable +#define RP2040_PWM_INTF_OFFSET 0xac // Interrupt Force +#define RP2040_PWM_INTS_OFFSET 0xb0 // Interrupt status after masking & forcing + +/* Timer register accessors */ +#define rCSR(_tmr) REG(_tmr,RP2040_PWM_CSR_OFFSET) +#define rDIV(_tmr) REG(_tmr,RP2040_PWM_DIV_OFFSET) +#define rCTR(_tmr) REG(_tmr,RP2040_PWM_CTR_OFFSET) +#define rCCR(_tmr) REG(_tmr,RP2040_PWM_CCR_OFFSET) +#define rTOP(_tmr) REG(_tmr,RP2040_PWM_TOP_OFFSET) +#define rEN _REG32(RP2040_PWM_BASE,RP2040_PWM_EN_OFFSET) +#define rINTR _REG32(RP2040_PWM_BASE,RP2040_PWM_INTR_OFFSET) +#define rINTE _REG32(RP2040_PWM_BASE,RP2040_PWM_INTE_OFFSET) +#define rINTF _REG32(RP2040_PWM_BASE,RP2040_PWM_INTF_OFFSET) +#define rINTS _REG32(RP2040_PWM_BASE,RP2040_PWM_INTS_OFFSET) + +// NotUsed PWMOut PWMIn Capture OneShot Trigger +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 }; + +typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ + +static io_timer_allocation_t once = 0; // Used to trace whether the timer is initialized or not + +typedef struct channel_stat_t { + uint32_t isr_cout; + uint32_t overflows; +} channel_stat_t; + +// static channel_stat_t io_timer_channel_stats[MAX_TIMER_IO_CHANNELS]; + +static struct channel_handler_entry { + channel_handler_t callback; + void *context; +} channel_handlers[MAX_TIMER_IO_CHANNELS]; + + +static int io_timer_handler(int irq, void *context, void *arg)//uint16_t timer_index) +{ + /* Read the count at the time of the interrupt */ + + // uint16_t count = rCNT(timer_index); + + // /* Read the HRT at the time of the interrupt */ + + // hrt_abstime now = hrt_absolute_time(); + + // const io_timers_t *tmr = &io_timers[timer_index]; + + // /* What is pending */ + + // uint32_t statusr = rSTATUS(timer_index); + + // /* Acknowledge all that are pending */ + + // rSTATUS(timer_index) = 0; + + // /* Iterate over the timer_io_channels table */ + + // uint32_t first_channel_index = io_timers_channel_mapping.element[timer_index].first_channel_index; + // uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer_index].channel_count; + + // for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + + // uint16_t chan = 1 << chan_index; + + // if (statusr & chan) { + + // io_timer_channel_stats[chan_index].isr_cout++; + + // /* Call the client to read the rCnV etc and clear the CHnF */ + + // if (channel_handlers[chan_index].callback) { + // channel_handlers[chan_index].callback(channel_handlers[chan_index].context, tmr, + // chan_index, &timer_io_channels[chan_index], + // now, count, _REG32(tmr->base, KINETIS_FTM_CV_OFFSET(chan_index))); + // } + // } + + // /* Did it set again during call out ?*/ + + // if (rSTATUS(timer_index) & chan) { + + // /* Error we has a second edge before we serviced the fist */ + + // io_timer_channel_stats[chan_index].overflows++; + // } + // } + + return 0; +} + +static inline int validate_timer_index(unsigned timer) +{ + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} + +static inline int is_timer_uninitalized(unsigned timer) +{ + int rv = 0; + + if (once & 1 << timer) { + rv = -EBUSY; + } + + return rv; +} + +static inline void set_timer_initalized(unsigned timer) +{ + once |= 1 << timer; +} + +static inline void set_timer_deinitalized(unsigned timer) +{ + once &= ~(1 << timer); +} + +static inline int channels_timer(unsigned channel) +{ + return timer_io_channels[channel].timer_index; +} + +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + + /* Gather the channel bits that belong to the timer */ + + uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; + uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; + + for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + +static inline int is_channels_timer_uninitalized(unsigned channel) +{ + return is_timer_uninitalized(channels_timer(channel)); +} + +int io_timer_is_channel_free(unsigned channel) +{ + int rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { + rv = -EBUSY; + } + } + + return rv; +} + +int io_timer_validate_channel_index(unsigned channel) +{ + int rv = -EINVAL; + + if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) { + + unsigned timer = timer_io_channels[channel].timer_index; + + /* test timer for validity */ + + if ((io_timers[timer].base != 0) && + (timer_io_channels[channel].gpio_out != 0) && + (timer_io_channels[channel].gpio_in != 0)) { + rv = 0; + } + } + + return rv; +} + +uint32_t io_timer_channel_get_gpio_output(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return timer_io_channels[channel].gpio_out; +} + +uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return timer_io_channels[channel].gpio_in; +} + + +int io_timer_get_mode_channels(io_timer_channel_mode_t mode) +{ + if (mode < IOTimerChanModeSize) { + return channel_allocations[mode]; + } + + return 0; +} + +int io_timer_get_channel_mode(unsigned channel) +{ + io_timer_channel_allocation_t bit = 1 << channel; + + for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) { + if (bit & channel_allocations[mode]) { + return mode; + } + } + + return -1; +} + +static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode_t mode, + io_timer_channel_mode_t new_mode) +{ + /* If caller mode is not based on current setting adjust it */ + + if ((channels & channel_allocations[IOTimerChanMode_NotUsed]) == channels) { + mode = IOTimerChanMode_NotUsed; + } + + /* Remove old set of channels from original */ + + channel_allocations[mode] &= ~channels; + + /* Will this change ?*/ + + uint32_t before = channel_allocations[new_mode] & channels; + + /* add in the new set */ + + channel_allocations[new_mode] |= channels; + + /* Indicate a mode change */ + + return before ^ channels; +} + +static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = io_timer_is_channel_free(channel); + + if (rv == 0) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; + channel_allocations[mode] |= bit; + } + + return rv; +} + + +static inline int free_channel_resource(unsigned channel) +{ + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_channel_allocation_t bit = 1 << channel; + channel_allocations[mode] &= ~bit; + channel_allocations[IOTimerChanMode_NotUsed] |= bit; + } + + return mode; +} + +int io_timer_free_channel(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + + int mode = io_timer_get_channel_mode(channel); + + if (mode > IOTimerChanMode_NotUsed) { + io_timer_set_enable(false, mode, 1 << channel); + free_channel_resource(channel); + + } + + return 0; +} + + +static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) +{ + int rv = -EINVAL; + + if (mode != IOTimerChanMode_NotUsed) { + rv = io_timer_validate_channel_index(channel); + + if (rv == 0) { + rv = allocate_channel_resource(channel, mode); + } + } + + return rv; +} + +static int timer_set_rate(unsigned timer, unsigned rate) +{ + // RP2040 has a buffer for rTOP, so there shouldn't be any need to turn the timer off to change rTOP value + rTOP(timer) = (BOARD_PWM_FREQ / rate) - 1; + + return 0; +} + +static inline uint32_t freq2div(uint32_t freq) +{ + return (TIM_SRC_CLOCK_FREQ << 4) / freq; +} + +static inline void io_timer_set_oneshot_mode(unsigned timer) +{ + /* Ideally, we would want per channel One pulse mode in HW + * Alas OPE stops the Timer not the channel + * todo:We can do this in an ISR later + * But since we do not have that + * We try to get the longest rate we can. + * On 16 bit timers this is 8.1 Ms. + */ + + rTOP(timer) = 0xffff; + rDIV(timer) = freq2div(BOARD_ONESHOT_FREQ); +} + +static inline void io_timer_set_PWM_mode(unsigned timer) +{ + rDIV(timer) = freq2div(BOARD_PWM_FREQ); +} + +void io_timer_trigger(void) +{ + // This function is probably not important for RP2040 as the buffered registers are updated automatically on the timer wrap. + + // int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + // uint32_t action_cache[MAX_IO_TIMERS] = {0}; + // int actions = 0; + + // /* Pre-calculate the list of timers to Trigger */ + + // for (int timer = 0; timer < MAX_IO_TIMERS; timer++) { + // if (validate_timer_index(timer) == 0) { + // int channels = get_timer_channels(timer); + + // if (oneshots & channels) { + // action_cache[actions++] = io_timers[timer].base; + // } + // } + // } + + // /* Now do them all wit the shortest delay in between */ + + // irqstate_t flags = px4_enter_critical_section(); + + // for (actions = 0; actions < MAX_IO_TIMERS && action_cache[actions] != 0; actions++) { + // _REG32(action_cache[actions], KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC; + // } + + // px4_leave_critical_section(flags); +} + +int io_timer_init_timer(unsigned timer) +{ + /* Do this only once per timer */ + + int rv = is_timer_uninitalized(timer); + + if (rv == 0) { + + irqstate_t flags = px4_enter_critical_section(); + + set_timer_initalized(timer); + + /* disable and configure the timer */ + + rCSR(timer) = 0; + rCTR(timer) = 0; + + /* enable the timer */ + + io_timer_set_PWM_mode(timer); + + /* + * Note we do the Standard PWM Out init here + * default to updating at 50Hz + */ + + timer_set_rate(timer, 50); + + /* + * Note that the timer is left disabled with IRQ subs installed + * and active but DEIR bits are not set. + */ + xcpt_t handler = io_timer_handler; + + if (handler) { + irq_attach(RP2040_PWM_IRQ_WRAP, handler, NULL); + up_enable_irq(RP2040_PWM_IRQ_WRAP); + } + + px4_leave_critical_section(flags); + } + + return rv; +} + + +int io_timer_set_rate(unsigned timer, unsigned rate) +{ + int rv = EBUSY; + + /* Get the channel bits that belong to the timer */ + + uint32_t channels = get_timer_channels(timer); + + /* Check that all channels are either in PWM or Oneshot */ + + if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | + channel_allocations[IOTimerChanMode_OneShot] | + channel_allocations[IOTimerChanMode_NotUsed])) == + channels) { + + /* Change only a timer that is owned by pwm or one shot */ + + /* Request to use OneShot ?*/ + + if (rate == 0) { + + /* Request to use OneShot + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be Oneshot + */ + + /* Did the allocation change */ + if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { + io_timer_set_oneshot_mode(timer); + } + + } else { + + /* Request to use PWM + * + * We are here because ALL these channels were either PWM or Oneshot + * Now they need to be PWM + */ + + if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { + io_timer_set_PWM_mode(timer); + } + + timer_set_rate(timer, rate); + } + + rv = OK; + } + + return rv; +} + +int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, + channel_handler_t channel_handler, void *context) +{ + // RP2040 doesn't have any specific configuration for each channel except the CCR value. + // And RP2040 doesn't really have the hardware capability for input capture. + + /* figure out the GPIO config first */ + switch (mode) { + + case IOTimerChanMode_OneShot: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_Trigger: + case IOTimerChanMode_NotUsed: + break; + + default: + return -EINVAL; + } + + int rv = allocate_channel(channel, mode); + + /* Valid channel should now be reserved in new mode */ + + if (rv >= 0) { + + /* Blindly try to initialize the timer - it will only do it once */ + + io_timer_init_timer(channels_timer(channel)); + + /* configure the channel */ // Nothing to configure here really. + + // uint32_t chan = timer_io_channels[channel].timer_channel - 1; + + channel_handlers[channel].callback = channel_handler; + channel_handlers[channel].context = context; + } + + return rv; +} + +int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks) +{ + typedef struct action_cache_rp_t { + // uint32_t cnsc_offset; + // uint32_t cnsc_value; + uint32_t gpio; + } action_cache_rp_t; + struct action_cache_t { + uint32_t base; + uint32_t index; + action_cache_rp_t cnsc[MAX_CHANNELS_PER_TIMER]; + } action_cache[MAX_IO_TIMERS]; + + memset(action_cache, 0, sizeof(action_cache)); + + // uint32_t bits = state ? CnSC_PWMOUT_INIT : 0; + + switch (mode) { + case IOTimerChanMode_NotUsed: + case IOTimerChanMode_PWMOut: + case IOTimerChanMode_OneShot: + case IOTimerChanMode_Trigger: + break; + + default: + return -EINVAL; + } + + /* Was the request for all channels in this mode ?*/ + + if (masks == IO_TIMER_ALL_MODES_CHANNELS) { + + /* Yes - we provide them */ + + masks = channel_allocations[mode]; + + } else { + + /* No - caller provided mask */ + + /* Only allow the channels in that mode to be affected */ + + masks &= channel_allocations[mode]; + + } + + /* Pre calculate all the changes */ + + for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) { + if (masks & (1 << chan_index)) { + masks &= ~(1 << chan_index); + + if (io_timer_validate_channel_index(chan_index) == 0) { + // uint32_t chan = timer_io_channels[chan_index].timer_channel - 1; + uint32_t timer = channels_timer(chan_index); + action_cache[timer].base = io_timers[timer].base; + // action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + KINETIS_FTM_CSC_OFFSET(chan); + // action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits; + + if ((state && + (mode == IOTimerChanMode_PWMOut || + mode == IOTimerChanMode_OneShot || + mode == IOTimerChanMode_Trigger))) { + action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out; + } + + action_cache[timer].index++; + } + } + } + + irqstate_t flags = px4_enter_critical_section(); + + + for (unsigned actions = 0; actions < arraySize(action_cache); actions++) { + // uint32_t any_on = 0; + + if (action_cache[actions].base != 0) { + for (unsigned int index = 0; index < action_cache[actions].index; index++) { + + if (action_cache[actions].cnsc[index].gpio) { + px4_arch_configgpio(action_cache[actions].cnsc[index].gpio); + } + + // _REG(action_cache[actions].cnsc[index].cnsc_offset) = action_cache[actions].cnsc[index].cnsc_value; + // any_on |= action_cache[actions].cnsc[index].cnsc_value; + } + + /* Any On ?*/ + + /* Assume not */ + // uint32_t regval = _REG32(action_cache[actions].base, KINETIS_FTM_SC_OFFSET); + // regval &= ~(FTM_SC_CLKS_MASK); + + // if (any_on != 0) { + + // /* force an update to preload all registers */ + // _REG32(action_cache[actions].base, KINETIS_FTM_SYNC_OFFSET) |= FTM_SYNC; + + // /* arm requires the timer be enabled */ + // regval |= (FTM_SC_CLKS_EXTCLK); + // } + + _REG32(action_cache[actions].base, RP2040_PWM_CSR_OFFSET) |= state; + } + } + + px4_leave_critical_section(flags); + + return 0; +} + +int io_timer_set_ccr(unsigned channel, uint16_t value) +{ + int rv = io_timer_validate_channel_index(channel); + int mode = io_timer_get_channel_mode(channel); + + if (rv == 0) { + if ((mode != IOTimerChanMode_PWMOut) && + (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Trigger)) { + + rv = -EIO; + + } else { + + /* configure the channel */ + int regVal = rCCR(channels_timer(channel)); + regVal &= ~(0xffff << (timer_io_channels[channel].timer_channel - 1) * 16); + regVal |= value << (timer_io_channels[channel].timer_channel - 1) * 16; + rCCR(channels_timer(channel)) = regVal; + } + } + + return rv; +} + +uint16_t io_channel_get_ccr(unsigned channel) +{ + uint16_t value = 0; + + if (io_timer_validate_channel_index(channel) == 0) { + int mode = io_timer_get_channel_mode(channel); + + if ((mode == IOTimerChanMode_PWMOut) || + (mode == IOTimerChanMode_OneShot) || + (mode == IOTimerChanMode_Trigger)) { + value = rCCR(channels_timer(channel)) >> (timer_io_channels[channel].timer_channel - 1) * 16; + } + } + + return value; +} + +uint32_t io_timer_get_group(unsigned timer) +{ + return get_timer_channels(timer); + +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c new file mode 100644 index 000000000000..b8ab8db721ea --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/pwm_servo.c @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * 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 drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to RP2040 PWM blocks. + * + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +int up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + return io_timer_set_ccr(channel, value); +} + +servo_position_t up_pwm_servo_get(unsigned channel) +{ + return io_channel_get_ccr(channel); +} + +int up_pwm_servo_init(uint32_t channel_mask) +{ + /* Init channels */ + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + + // First free the current set of PWMs + + for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (current & (1 << channel)) { + io_timer_free_channel(channel); + current &= ~(1 << channel); + } + } + + // Now allocate the new set + + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not PWM mode before + + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + } + } + + return OK; +} + +void up_pwm_servo_deinit(uint32_t channel_mask) +{ + /* disable the timers */ + up_pwm_servo_arm(false, channel_mask); +} + +int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) +{ + if ((group >= MAX_IO_TIMERS) || (io_timers[group].base == 0)) { + return ERROR; + } + + /* Allow a rate of 0 to enter oneshot mode */ + + if (rate != 0) { + + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + + if (rate < 1) { + return -ERANGE; + } + + if (rate > 10000) { + return -ERANGE; + } + } + + return io_timer_set_rate(group, rate); +} + +void up_pwm_update(unsigned channels_mask) +{ + io_timer_trigger(); +} + +int up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { + up_pwm_servo_set_rate_group_update(i, rate); + } + + return 0; +} + +uint32_t up_pwm_servo_get_rate_group(unsigned group) +{ + return io_timer_get_group(group); +} + +void +up_pwm_servo_arm(bool armed, uint32_t channel_mask) +{ + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c new file mode 100644 index 000000000000..04865ce17935 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/io_pins/rp2040_pinset.c @@ -0,0 +1,47 @@ +#include +#include + +#include + +#include +#include + +int rp2040_gpioconfig(uint32_t pinset) +{ + if ((pinset & GPIO_NUM_MASK) > RP2040_GPIO_NUM) { + return -EINVAL; + } + + rp2040_gpio_set_pulls(pinset & GPIO_NUM_MASK, pinset & GPIO_PU_MASK, pinset & GPIO_PD_MASK); + + if ((pinset & GPIO_FUN_MASK) >> 9 == RP2040_GPIO_FUNC_SIO) { + rp2040_gpio_setdir(pinset & GPIO_NUM_MASK, pinset & GPIO_OUT_MASK); + rp2040_gpio_put(pinset & GPIO_NUM_MASK, pinset & GPIO_SET_MASK); + } + + rp2040_gpio_set_function(pinset & GPIO_NUM_MASK, (pinset & GPIO_FUN_MASK) >> 9); + + return OK; +} + +// Be careful when using this function. Current nuttx implementation allows for only one type of interrupt +// (out of four types rising, falling, level high, level low) to be active at a time. +int rp2040_setgpioevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg) +{ + int ret = -ENOSYS; + + if (fallingedge & event & (func != NULL)) { + ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, func, arg); + rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK); + + } else if (risingedge & event & (func != NULL)) { + ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_HIGH, func, arg); + rp2040_gpio_enable_irq(pinset & GPIO_NUM_MASK); + + } else { + rp2040_gpio_disable_irq(pinset & GPIO_NUM_MASK); + ret = rp2040_gpio_irq_attach(pinset & GPIO_NUM_MASK, RP2040_GPIO_INTR_EDGE_LOW, NULL, NULL); + } + + return ret; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt new file mode 100644 index 000000000000..dda6d7c98ebd --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/spi/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_spi + spi.cpp +) +target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp b/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp new file mode 100644 index 000000000000..350b91434271 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/spi/spi.cpp @@ -0,0 +1,329 @@ +/**************************************************************************** + * + * 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 +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +static const px4_spi_bus_t *_spi_bus0; +static const px4_spi_bus_t *_spi_bus1; + +static void spi_bus_configgpio_cs(const px4_spi_bus_t *bus) +{ + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio != 0) { + px4_arch_configgpio(bus->devices[i].cs_gpio | GPIO_FUN(RP2040_GPIO_FUNC_SIO)); + } + } +} + +__EXPORT void rp2040_spiinitialize() +{ + px4_set_spi_buses_from_hw_version(); + board_control_spi_sensors_power_configgpio(); + board_control_spi_sensors_power(true, 0xffff); + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case PX4_BUS_NUMBER_TO_PX4(0): _spi_bus0 = &px4_spi_buses[i]; break; + + case PX4_BUS_NUMBER_TO_PX4(1): _spi_bus1 = &px4_spi_buses[i]; break; + } + } + + /* Set default SPI pin */ +#if defined(CONFIG_RP2040_SPI0) && defined(GPIO_SPI0_SCLK) && defined(GPIO_SPI0_MISO) && defined(GPIO_SPI0_MOSI) + px4_arch_configgpio(GPIO_SPI0_SCLK); + px4_arch_configgpio(GPIO_SPI0_MISO); + px4_arch_configgpio(GPIO_SPI0_MOSI); +#endif + +#if defined(CONFIG_RP2040_SPI1) && defined(GPIO_SPI1_SCLK) && defined(GPIO_SPI1_MISO) && defined(GPIO_SPI1_MOSI) + px4_arch_configgpio(GPIO_SPI1_SCLK); + px4_arch_configgpio(GPIO_SPI1_MISO); + px4_arch_configgpio(GPIO_SPI1_MOSI); +#endif + +#ifdef CONFIG_RP2040_SPI0 + ASSERT(_spi_bus0); + + if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(0))) { + spi_bus_configgpio_cs(_spi_bus0); + } + +#endif // CONFIG_RP2040_SPI0 + +#ifdef CONFIG_RP2040_SPI1 + ASSERT(_spi_bus1); + + if (board_has_bus(BOARD_SPI_BUS, PX4_BUS_NUMBER_TO_PX4(1))) { + spi_bus_configgpio_cs(_spi_bus1); + } + +#endif // CONFIG_RP2040_SPI1 +} + +static inline void rp2040_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 + px4_arch_gpiowrite(bus->devices[i].cs_gpio, !selected); + } + } +} + + +/**************************************************************************** + * Name: rp2040_spi0/1select and rp2040_spi0/1status + * + * Description: + * The external functions, rp2040_spi0/1select and rp2040_spi0/1status + * 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 rp2040_spibus_initialize()) are provided by + * common RP2040 logic. To use this common SPI logic on your board: + * + * 1. Provide logic in rp2040_boardinitialize() to configure SPI chip + * select pins. + * 2. Provide rp2040_spi0/1select() and rp2040_spi0/1status() + * 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 rp2040_spibus_initialize() in your low level + * application initialization logic + * 4. The handle returned by rp2040_spibus_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). + * + ****************************************************************************/ +#ifdef CONFIG_RP2040_SPI0 +void rp2040_spi0select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, + selected ? "assert" : "de-assert"); + rp2040_spixselect(_spi_bus0, dev, devid, selected); +} + +uint8_t rp2040_spi0status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_RP2040_SPI1 +void rp2040_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, + bool selected) +{ + spiinfo("devid: %d CS: %s\n", (int)devid, + selected ? "assert" : "de-assert"); + rp2040_spixselect(_spi_bus1, dev, devid, selected); +} + +uint8_t rp2040_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +void board_control_spi_sensors_power(bool enable_power, int bus_mask) +{ + const px4_spi_bus_t *buses = px4_spi_buses; + + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (buses[bus].bus == -1) { + break; + } + + const bool bus_matches = bus_mask & (1 << (buses[bus].bus - 1)); + + if (buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, buses[bus].bus) || + !bus_matches) { + continue; + } + + px4_arch_gpiowrite(buses[bus].power_enable_gpio, enable_power ? 1 : 0); + } +} + +void board_control_spi_sensors_power_configgpio() +{ + const px4_spi_bus_t *buses = px4_spi_buses; + + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (buses[bus].bus == -1) { + break; + } + + if (buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, buses[bus].bus)) { + continue; + } + + px4_arch_configgpio(buses[bus].power_enable_gpio); + } +} + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ + bool has_power_enable = false; + + // disable SPI bus + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == -1) { + break; + } + + const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1)); + + if (px4_spi_buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) || + !bus_requested) { + continue; + } + + has_power_enable = true; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio)); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + px4_arch_configgpio(PX4_GPIO_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio)); + } + } + +#if defined(CONFIG_RP2040_SPI0) + + if (px4_spi_buses[bus].bus == 1) { + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_SCLK)); + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MISO)); + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI0_MOSI)); + } + +#endif +#if defined(CONFIG_RP2040_SPI1) + + if (px4_spi_buses[bus].bus == 2) { + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_SCLK)); + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MISO)); + px4_arch_configgpio(PX4_GPIO_PIN_OFF(GPIO_SPI1_MOSI)); + } + +#endif + } + + if (!has_power_enable) { + // board does not have power control over any of the sensor buses + return; + } + + // set the sensor rail(s) off + board_control_spi_sensors_power(false, bus_mask); + + // wait for the sensor rail to reach GND + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + // switch the sensor rail back on + board_control_spi_sensors_power(true, bus_mask); + + /* 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) { + break; + } + + const bool bus_requested = bus_mask & (1 << (px4_spi_buses[bus].bus - 1)); + + if (px4_spi_buses[bus].power_enable_gpio == 0 || + !board_has_bus(BOARD_SPI_BUS, px4_spi_buses[bus].bus) || + !bus_requested) { + continue; + } + + 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); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].drdy_gpio); + } + } + +#if defined(CONFIG_RP2040_SPI0) + + if (px4_spi_buses[bus].bus == 1) { + px4_arch_configgpio(GPIO_SPI0_SCLK); + px4_arch_configgpio(GPIO_SPI0_MISO); + px4_arch_configgpio(GPIO_SPI0_MOSI); + } + +#endif +#if defined(CONFIG_RP2040_SPI1) + + if (px4_spi_buses[bus].bus == 2) { + px4_arch_configgpio(GPIO_SPI1_SCLK); + px4_arch_configgpio(GPIO_SPI1_MISO); + px4_arch_configgpio(GPIO_SPI1_MOSI); + } + +#endif + } +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt b/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt new file mode 100644 index 000000000000..b102ab612f1b --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(arch_version + board_identity.c + board_mcu_version.c +) diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c new file mode 100644 index 000000000000..677c3eb8a141 --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c @@ -0,0 +1,146 @@ +/** + * @file board_identity.c + * Implementation of RP2040 based Board identity API + */ + +#include +#include +#include + +// RP2040 doesn't really have a cpu register with unique id. +// However, there is a function in pico-sdk which can provide +// a device unique id from its flash which is 64 bits in length. +// For now, a fixed value of 12 bytes "PIPICORP2040" is used. +uint32_t myUUID[3] = {'P' << 0 | 'I' << 8 | 'P' << 16 | 'I' << 24, + 'C' << 0 | 'O' << 8 | 'R' << 16 | 'P' << 24, + '2' << 0 | '0' << 8 | '4' << 16 | '0' << 24 + }; +#define RP2040_SYSTEM_UID ((uint32_t)myUUID) + +#define CPU_UUID_BYTE_FORMAT_ORDER {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8} +#define SWAP_UINT32(x) (((x) >> 24) | (((x) & 0x00ff0000) >> 8) | (((x) & 0x0000ff00) << 8) | ((x) << 24)) + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID; + +/* A type suitable for holding the reordering array for the byte format of the UUID + */ + +typedef const uint8_t uuid_uint8_reorder_t[PX4_CPU_UUID_BYTE_LENGTH]; + + +void board_get_uuid(uuid_byte_t uuid_bytes) +{ + uuid_uint8_reorder_t reorder = CPU_UUID_BYTE_FORMAT_ORDER; + union { + uuid_byte_t b; + uuid_uint32_t w; + } id; + + /* Copy the serial from the chips non-write memory */ + + board_get_uuid32(id.w); + + /* swap endianess */ + + for (int i = 0; i < PX4_CPU_UUID_BYTE_LENGTH; i++) { + uuid_bytes[i] = id.b[reorder[i]]; + } +} + +__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) +{ + uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID; + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uuid_words[i] = chip_uuid[i]; + } +} + +int board_get_uuid32_formated(char *format_buffer, int size, + const char *format, + const char *seperator) +{ + uuid_uint32_t uuid; + board_get_uuid32(uuid); + int offset = 0; + int sep_size = seperator ? strlen(seperator) : 0; + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]); + + if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) { + strcat(&format_buffer[offset], seperator); + offset += sep_size; + } + } + + return 0; +} + +int board_get_mfguid(mfguid_t mfgid) +{ + uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID; + uint8_t *rv = &mfgid[0]; + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + memcpy(rv, &uuid_bytes, sizeof(uint32_t)); + rv += sizeof(uint32_t); + } + + return PX4_CPU_MFGUID_BYTE_LENGTH; +} + +int board_get_mfguid_formated(char *format_buffer, int size) +{ + mfguid_t mfguid; + + board_get_mfguid(mfguid); + int offset = 0; + + for (unsigned i = 0; offset < size && i < PX4_CPU_MFGUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", mfguid[i]); + } + + return offset; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + for (unsigned i = 0; i < PX4_GUID_BYTE_LENGTH - (sizeof(soc_arch_id) + PX4_CPU_UUID_BYTE_LENGTH); i++) { + *pb++ = 0; + } + + uint32_t *chip_uuid = (uint32_t *) RP2040_SYSTEM_UID; + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uint32_t uuid_bytes = SWAP_UINT32(chip_uuid[(PX4_CPU_UUID_WORD32_LENGTH - 1) - i]); + memcpy(pb, &uuid_bytes, sizeof(uint32_t)); + pb += sizeof(uint32_t); + } + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c new file mode 100644 index 000000000000..646eab9185db --- /dev/null +++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_mcu_version.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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_mcu_version.c + * Implementation of RP2040 based SoC version API + */ + +#include +#include + +#define RP2040_CPUID_BASE (RP2040_PPB_BASE + 0xed00) + +/* magic numbers from reference manual */ + +enum MCU_REV { + MCU_REV_RP2040_REV_1 = 0x1 +}; + +/* Define any issues with the Silicon as lines separated by \n + * omitting the last \n + */ +#define RP2040_ERRATA "This device does not have a unique id!" + + +// RP2040 datasheet CPUID register +# define REVID_MASK 0xF +# define DEVID_MASK 0xFFFFFFF0 + +# define RP2040_DEVICE_ID 0x410CC60 + + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + uint32_t abc = getreg32(RP2040_CPUID_BASE); + + int32_t chip_version = (abc & DEVID_MASK) > 4; + enum MCU_REV revid = abc & REVID_MASK; + const char *chip_errata = NULL; + + switch (chip_version) { + + + case RP2040_DEVICE_ID: + *revstr = "RP2040"; + chip_errata = RP2040_ERRATA; + break; + + default: + *revstr = "RPI???"; + break; + } + + switch (revid) { + + case MCU_REV_RP2040_REV_1: + *rev = '1'; + break; + + default: + *rev = '?'; + revid = -1; + break; + } + + if (errata) { + *errata = chip_errata; + } + + return revid; +} diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt index 3a466715b1fb..2d3a79965455 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt @@ -34,3 +34,4 @@ px4_add_library(arch_board_hw_info board_hw_rev_ver.c ) +target_link_libraries(arch_board_hw_info PRIVATE arch_adc) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index c6a7ca549200..11e699a5163d 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -170,7 +170,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Turn the sense lines to digital outputs LOW */ - stm32_configgpio(PX4_MAKE_GPIO_OUTPUT(gpio_sense)); + stm32_configgpio(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense)); up_udelay(100); /* About 10 TC assuming 485 K */ @@ -182,7 +182,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Write the sense lines HIGH */ - stm32_gpiowrite(PX4_MAKE_GPIO_OUTPUT(gpio_sense), 1); + stm32_gpiowrite(PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio_sense), 1); up_udelay(100); /* About 10 TC assuming 485 K */ @@ -203,7 +203,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Are Resistors in place ?*/ uint32_t dn_sum = 0; - uint16_t dn = 0; + uint32_t dn = 0; if ((high ^ low) && low == 0) { @@ -216,14 +216,14 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc for (unsigned av = 0; av < samples; av++) { dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel); - if (dn == 0xffff) { + if (dn == UINT32_MAX) { break; } dn_sum += dn; } - if (dn != 0xffff) { + if (dn != UINT32_MAX) { *id = dn_sum / samples; rv = OK; } @@ -264,9 +264,9 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc */ uint32_t dn_sum = 0; - uint16_t dn = 0; - uint16_t high = 0; - uint16_t low = 0; + uint32_t dn = 0; + uint32_t high = 0; + uint32_t low = 0; /* Turn the drive lines to digital outputs High */ @@ -278,7 +278,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc if (px4_arch_adc_init(HW_REV_VER_ADC_BASE) == OK) { dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel); - if (dn == 0xffff) { + if (dn == UINT32_MAX) { break; } @@ -286,7 +286,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc } } - if (dn != 0xffff) { + if (dn != UINT32_MAX) { high = dn_sum / samples; } @@ -302,18 +302,18 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel); - if (dn == 0xffff) { + if (dn == UINT32_MAX) { break; } dn_sum += dn; } - if (dn != 0xffff) { + if (dn != UINT32_MAX) { low = dn_sum / samples; } - if ((high > low) && high > px4_arch_adc_dn_fullcount() - 100) { + if ((high > low) && high > ((px4_arch_adc_dn_fullcount() * 975) / 1000)) { *id = low; rv = OK; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/CMakeLists.txt index f2716046102a..06ccd082037c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/CMakeLists.txt @@ -34,3 +34,10 @@ px4_add_library(arch_board_reset board_reset.cpp ) + +# up_systemreset +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp index c5bf204779c1..15dfbd87616a 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp @@ -65,7 +65,7 @@ static const uint32_t modes[] = { /* to tb */ /* BOARD_RESET_MODE_CLEAR 5 y */ 0, - /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb0070001, + /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb007b007, /* BOARD_RESET_MODE_BOOT_TO_VALID_APP 0 y */ 0xb0070002, /* BOARD_RESET_MODE_CAN_BL 10 n */ 0xb0080000, /* BOARD_RESET_MODE_RTC_BOOT_FWOK 0 n */ 0xb0093a26 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index dbb53386234e..d9b8d013b0ec 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2019, 2021 PX4 Development Team. All rights reserved. * Author: Igor Misic * * Redistribution and use in source and binary forms, with or without @@ -78,41 +78,29 @@ typedef struct dshot_handler_t { #define DSHOT_BURST_BUFFER_SIZE(motors_number) (DMA_ALIGN_UP(sizeof(uint32_t)*ONE_MOTOR_BUFF_SIZE*motors_number)) static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; -static uint16_t *motor_buffer = NULL; static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] -px4_cache_aligned_data(); +px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; -#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT -static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT; -#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ - -void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number); - int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { - // Alloc buffers if they do not exist. We don't use channel_mask so that potential future re-init calls can - // use the same buffer. - if (!motor_buffer) { - motor_buffer = (uint16_t *)malloc(sizeof(uint16_t) * ALL_MOTORS_BUF_SIZE); + unsigned buffer_offset = 0; - if (!motor_buffer) { - return -ENOMEM; - } + for (int timer_index = 0; timer_index < DSHOT_TIMERS; timer_index++) { + dshot_handler[timer_index].init = false; } - unsigned buffer_offset = 0; - for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { if (io_timers[timer].base == 0) { // no more timers configured break; } // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size +#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-align" dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; #pragma GCC diagnostic pop - buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count); + buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps); if (buffer_offset > sizeof(dshot_burst_buffer_array)) { return -EINVAL; // something is wrong with the board configuration or some other logic @@ -121,6 +109,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) /* Init channels */ int ret_val = OK; + int channels_init_mask = 0; for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { if (channel_mask & (1 << channel)) { @@ -130,16 +119,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) continue; } - // First free any that were not DShot mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); + channel_mask &= ~(1 << channel); if (OK == ret_val) { - channel_mask &= ~(1 << channel); dshot_handler[timer].init = true; + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; } } } @@ -147,9 +136,10 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { if (true == dshot_handler[timer_index].init) { - dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count * + dshot_handler[timer_index].dma_size = io_timers_channel_mapping.element[timer_index].channel_count_including_gaps * ONE_MOTOR_BUFF_SIZE; - io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, io_timers_channel_mapping.element[timer_index].channel_count); + io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, + io_timers_channel_mapping.element[timer_index].channel_count_including_gaps); dshot_handler[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dmamap); @@ -159,25 +149,19 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) } } - return ret_val; + return ret_val == OK ? channels_init_mask : ret_val; } void up_dshot_trigger(void) { - uint8_t first_motor = 0; - for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { if (true == dshot_handler[timer].init) { - uint8_t motors_number = io_timers_channel_mapping.element[timer].channel_count; - dshot_dmar_data_prepare(timer, first_motor, motors_number); - // Flush cache so DMA sees the data up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], - (uintptr_t)dshot_burst_buffer[timer] + DSHOT_BURST_BUFFER_SIZE(motors_number)); - - first_motor += motors_number; + (uintptr_t)dshot_burst_buffer[timer] + + DSHOT_BURST_BUFFER_SIZE(io_timers_channel_mapping.element[timer].channel_count_including_gaps)); px4_stm32_dmasetup(dshot_handler[timer].dma_handle, io_timers[timer].base + STM32_GTIM_DMAR_OFFSET, @@ -200,59 +184,36 @@ void up_dshot_trigger(void) * bit 12 - dshot telemetry enable/disable * bits 13-16 - XOR checksum **/ -static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry) { uint16_t packet = 0; uint16_t checksum = 0; -#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT - motor_number = motor_assignment[motor_number]; -#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ - packet |= throttle << DSHOT_THROTTLE_POSITION; packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; - uint32_t i; uint16_t csum_data = packet; /* XOR checksum calculation */ csum_data >>= NIBBLES_SIZE; - for (i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + for (unsigned i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { checksum ^= (csum_data & 0x0F); // XOR data by nibbles csum_data >>= NIBBLES_SIZE; } packet |= (checksum & 0x0F); - for (i = 0; i < ONE_MOTOR_DATA_SIZE; i++) { - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + i] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : - MOTOR_PWM_BIT_0; // MSB first - packet <<= 1; - } - - motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0; -} - -void up_dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) -{ - dshot_motor_data_set(motor_number, throttle + DShot_cmd_MIN_throttle, telemetry); -} - -void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) -{ - dshot_motor_data_set(channel, command, telemetry); -} - -void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number) -{ + unsigned timer = timer_io_channels[motor_number].timer_index; uint32_t *buffer = dshot_burst_buffer[timer]; + const io_timers_channel_mapping_element_t *mapping = &io_timers_channel_mapping.element[timer]; + unsigned num_motors = mapping->channel_count_including_gaps; + unsigned timer_channel_index = timer_io_channels[motor_number].timer_channel - mapping->lowest_timer_channel; - for (uint32_t motor_data_index = 0; motor_data_index < ONE_MOTOR_BUFF_SIZE ; motor_data_index++) { - for (uint32_t motor_index = 0; motor_index < motors_number; motor_index++) { - buffer[motor_data_index * motors_number + motor_index] = motor_buffer[(motor_index + - first_motor) * ONE_MOTOR_BUFF_SIZE + motor_data_index]; - } + for (unsigned motor_data_index = 0; motor_data_index < ONE_MOTOR_DATA_SIZE; motor_data_index++) { + buffer[motor_data_index * num_motors + timer_channel_index] = + (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first + packet <<= 1; } } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c index a88a973f69b0..025e31a0033b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c @@ -72,6 +72,33 @@ # define hrtinfo(x...) #endif +#if !defined(CONFIG_BUILD_FLAT) +#include +#include +#include + +#define HRT_ENTRY_QUEUE_MAX_SIZE 3 +static px4_sem_t g_wait_sem; +static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE]; +static int hrt_entry_queued = 0; +static bool suppress_entry_queue_error = false; +static bool hrt_entry_queue_error = false; + +void hrt_usr_call(void *arg) +{ + // This is called from hrt interrupt + if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) { + next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg; + + } else { + hrt_entry_queue_error = true; + } + + px4_sem_post(&g_wait_sem); +} + +#endif + #ifdef HRT_TIMER /* HRT configuration */ @@ -275,6 +302,8 @@ static void hrt_call_enter(struct hrt_call *entry); static void hrt_call_reschedule(void); static void hrt_call_invoke(void); + +int hrt_ioctl(unsigned int cmd, unsigned long arg); /* * Specific registers and bits used by PPM sub-functions */ @@ -694,46 +723,6 @@ hrt_absolute_time(void) return abstime; } -/** - * Convert a timespec to absolute time - */ -hrt_abstime -ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/** - * Convert absolute time to a timespec. - */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - -/** - * Compare a time value with the current time as atomic operation - */ -hrt_abstime -hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - irqstate_t flags = px4_enter_critical_section(); - - hrt_abstime delta = hrt_absolute_time() - *then; - - px4_leave_critical_section(flags); - - return delta; -} - /** * Store the absolute time in an interrupt-safe fashion */ @@ -758,6 +747,16 @@ hrt_init(void) /* configure the PPM input pin */ px4_arch_configgpio(GPIO_PPM_IN); #endif + +#if !defined(CONFIG_BUILD_FLAT) + /* Create a semaphore for handling hrt driver callbacks */ + px4_sem_init(&g_wait_sem, 0, 0); + /* this is a signalling semaphore */ + px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE); + + /* register ioctl callbacks */ + px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl); +#endif } /** @@ -1003,4 +1002,96 @@ hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) entry->deadline = hrt_absolute_time() + delay; } +#if !defined(CONFIG_BUILD_FLAT) +/* These functions are inlined in all but NuttX protected/kernel builds */ + +latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx) +{ + latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]}; + return ret; +} + +void reset_latency_counters(void) +{ + for (int i = 0; i <= get_latency_bucket_count(); i++) { + latency_counters[i] = 0; + } +} + +/* board_ioctl interface for user-space hrt driver */ +int +hrt_ioctl(unsigned int cmd, unsigned long arg) +{ + hrt_boardctl_t *h = (hrt_boardctl_t *)arg; + + switch (cmd) { + case HRT_WAITEVENT: { + irqstate_t flags; + px4_sem_wait(&g_wait_sem); + /* Atomically update the pointer to user side hrt entry */ + flags = px4_enter_critical_section(); + + /* This should be always true, but check it anyway */ + if (hrt_entry_queued > 0) { + *(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued]; + next_hrt_entry[hrt_entry_queued] = NULL; + + } else { + hrt_entry_queue_error = true; + } + + px4_leave_critical_section(flags); + + /* Warn once for entry queue being full */ + if (hrt_entry_queue_error && !suppress_entry_queue_error) { + PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued); + suppress_entry_queue_error = true; + } + } + break; + + case HRT_ABSOLUTE_TIME: + *(hrt_abstime *)arg = hrt_absolute_time(); + break; + + case HRT_CALL_AFTER: + hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + break; + + case HRT_CALL_AT: + hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + break; + + case HRT_CALL_EVERY: + hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry); + break; + + case HRT_CANCEL: + if (h && h->entry) { + hrt_cancel(h->entry); + + } else { + PX4_ERR("HRT_CANCEL called with NULL entry"); + } + + break; + + case HRT_GET_LATENCY: { + latency_boardctl_t *latency = (latency_boardctl_t *)arg; + latency->latency = get_latency(latency->bucket_idx, latency->counter_idx); + } + break; + + case HRT_RESET_LATENCY: + reset_latency_counters(); + break; + + default: + return -EINVAL; + } + + return OK; +} +#endif + #endif /* HRT_TIMER */ diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h index 2fda992be2f8..c28fa7e86cf9 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h @@ -91,6 +91,15 @@ enum Timer { Timer12, Timer13, Timer14, +#ifdef STM32_TIM15_BASE + Timer15, +#endif +#ifdef STM32_TIM16_BASE + Timer16, +#endif +#ifdef STM32_TIM17_BASE + Timer17 +#endif }; enum Channel { Channel1 = 1, @@ -153,6 +162,21 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer) case Timer::Timer14: return STM32_TIM14_BASE; #endif +#ifdef STM32_TIM15_BASE + + case Timer::Timer15: return STM32_TIM15_BASE; +#endif + +#ifdef STM32_TIM16_BASE + + case Timer::Timer16: return STM32_TIM16_BASE; +#endif + +#ifdef STM32_TIM17_BASE + + case Timer::Timer17: return STM32_TIM17_BASE; +#endif + default: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/i2c_hw_description.h index 2e9e55f44355..4b7940e1dd1b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/i2c_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/i2c_hw_description.h @@ -36,6 +36,7 @@ #include #include +#if defined(CONFIG_I2C) static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) { @@ -52,3 +53,4 @@ static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) ret.is_external = true; return ret; } +#endif // CONFIG_I2C 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 bda44530bb98..3f5d6676b760 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 @@ -74,6 +74,9 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_PPS = 8, + IOTimerChanMode_Other = 9, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -99,6 +102,8 @@ typedef struct io_timers_t { typedef struct io_timers_channel_mapping_element_t { uint32_t first_channel_index; uint32_t channel_count; + uint32_t lowest_timer_channel; + uint32_t channel_count_including_gaps; } io_timers_channel_mapping_element_t; /* mapping for each io_timers to timer_io_channels */ @@ -129,28 +134,33 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; -__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; - __EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context); -__EXPORT int io_timer_init_timer(unsigned timer); +__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_pwm_rate(unsigned timer, unsigned rate); __EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); __EXPORT uint16_t io_channel_get_ccr(unsigned channel); __EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value); __EXPORT uint32_t io_timer_get_group(unsigned timer); __EXPORT int io_timer_validate_channel_index(unsigned channel); -__EXPORT int io_timer_is_channel_free(unsigned channel); -__EXPORT int io_timer_free_channel(unsigned channel); +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode); +__EXPORT int io_timer_unallocate_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); -__EXPORT extern void io_timer_trigger(void); +__EXPORT extern void io_timer_trigger(unsigned channels_mask); __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); +/** + * Reserve a timer + * @return 0 on success (if not used yet, or already set to the mode) + */ +__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode); + +__EXPORT int io_timer_unallocate_timer(unsigned timer); + __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); /** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h index 75851d5a893a..55ea439ef73f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer_hw_description.h @@ -43,6 +43,7 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerChannel timer, GPIO::GPIOPin pin); +#define initIOTimerChannelCapture initIOTimerChannel // alias, used for param metadata generation static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t io_timers_conf[MAX_IO_TIMERS], Timer::TimerChannel timer, GPIO::GPIOPin pin) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h index 63803b5a4ad7..f2ef2854e69b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h @@ -103,7 +103,9 @@ __BEGIN_DECLS #define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) stm32_gpiosetevent(pinset,r,f,e,fp,a) #define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP)) -#define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR)) +#define PX4_MAKE_GPIO_EXTI(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_EXTI|GPIO_INPUT|GPIO_PULLUP)) +#define PX4_MAKE_GPIO_OUTPUT_CLEAR(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR)) +#define PX4_MAKE_GPIO_OUTPUT_SET(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET)) #define PX4_GPIO_PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_2MHz)) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index c51404312006..964627c636df 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -114,7 +114,7 @@ class ArchPX4IOSerial : public PX4IO_serial ArchPX4IOSerial(); ArchPX4IOSerial(ArchPX4IOSerial &) = delete; ArchPX4IOSerial &operator = (const ArchPX4IOSerial &) = delete; - ~ArchPX4IOSerial(); + virtual ~ArchPX4IOSerial(); virtual int init(); virtual int ioctl(unsigned operation, unsigned &arg); @@ -159,7 +159,6 @@ class ArchPX4IOSerial : public PX4IO_serial /** * Performance counters. */ - perf_counter_t _pc_dmasetup; perf_counter_t _pc_dmaerrs; /** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h index 1a3eb8a4464c..40cc40854b7c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h @@ -36,6 +36,7 @@ #include #include +#if defined(CONFIG_SPI) #include @@ -69,16 +70,22 @@ static inline constexpr px4_spi_bus_t initSPIBus(SPI::Bus bus, const px4_spi_bus for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { ret.devices[i] = devices.devices[i]; - // check that the same device is configured only once (the chip-select code depends on that) - for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) { - if (ret.devices[j].cs_gpio != 0) { - constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times"); - } - } if (ret.devices[i].cs_gpio != 0) { - // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) - if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + if (PX4_SPI_DEVICE_ID == PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + int same_devices_count = 0; + + for (int j = 0; j < i; ++j) { + if (ret.devices[j].cs_gpio != 0) { + same_devices_count += (ret.devices[i].devid & 0xff) == (ret.devices[j].devid & 0xff); + } + } + + // increment the 2. LSB byte to allow multiple devices of the same type + ret.devices[i].devid |= same_devices_count << 8; + + } else { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) ret.requires_locking = true; } } @@ -129,7 +136,8 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI struct px4_spi_bus_array_t { px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; }; -static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, const px4_spi_bus_array_t &bus_items) +static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, + const px4_spi_bus_array_t &bus_items) { px4_spi_bus_all_hw_t ret{}; @@ -137,7 +145,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, co ret.buses[i] = bus_items.item[i]; } - ret.board_hw_version = hw_version; + ret.board_hw_version_revision = hw_version_revision; return ret; } constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); @@ -161,3 +169,5 @@ constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD return false; } + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt index 6314afc0a52d..fd2a22debbb5 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt @@ -38,3 +38,5 @@ px4_add_library(arch_io_pins pwm_trigger.c ) target_compile_options(arch_io_pins PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries(arch_io_pins PRIVATE drivers_board) # timer_io_channels diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index 21989a1d4456..61fd70ee886c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -77,7 +77,7 @@ #include #include -#if defined(BOARD_HAS_CAPTURE) +#if !defined(BOARD_HAS_NO_CAPTURE) /* Support Input capture */ @@ -106,11 +106,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, uint16_t capture = _REG32(timer->base, chan->ccr_offset); channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = _REG32(timer->base, STM32_GTIM_SR_OFFSET) & chan->masks & GTIM_SR_CCOF; @@ -162,10 +162,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } else { - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - input_capture_bind(channel, callback, context); rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); @@ -501,4 +497,4 @@ int up_input_capture_get_stats(unsigned channel, input_capture_stats_t *stats, b return rv; } -#endif // defined(BOARD_HAS_CAPTURE) +#endif // !defined(BOARD_HAS_NO_CAPTURE) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 7cb2ec778a46..5a72cc9f5e30 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -149,14 +149,14 @@ static int io_timer_handler7(int irq, void *context, void *arg); /* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ #define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U -// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED PPS Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ -static io_timer_allocation_t once = 0; +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; -#if defined(BOARD_HAS_CAPTURE) +#if !defined(BOARD_HAS_NO_CAPTURE) /* Stats and handlers are only useful for Capture */ @@ -171,12 +171,12 @@ static struct channel_handler_entry { channel_handler_t callback; void *context; } channel_handlers[MAX_TIMER_IO_CHANNELS]; -#endif // defined(BOARD_HAS_CAPTURE) +#endif // !defined(BOARD_HAS_NO_CAPTURE) static int io_timer_handler(uint16_t timer_index) { -#if defined(BOARD_HAS_CAPTURE) +#if !defined(BOARD_HAS_NO_CAPTURE) /* Read the count at the time of the interrupt */ uint16_t count = rCNT(timer_index); @@ -231,7 +231,7 @@ static int io_timer_handler(uint16_t timer_index) /* Clear all the SR bits for interrupt enabled channels only */ rSR(timer_index) = ~(statusr & (enabled | enabled << 8)); -#endif // defined(BOARD_HAS_CAPTURE) +#endif // !defined(BOARD_HAS_NO_CAPTURE) return 0; } @@ -280,25 +280,34 @@ static inline int validate_timer_index(unsigned timer) return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; } -static inline int is_timer_uninitalized(unsigned timer) +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) { - int rv = 0; + int ret = -EINVAL; - if (once & 1 << timer) { - rv = -EBUSY; + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; + + } else { + ret = -EBUSY; + } } - return rv; + return ret; } -static inline void set_timer_initalized(unsigned timer) +int io_timer_unallocate_timer(unsigned timer) { - once |= 1 << timer; -} + int ret = -EINVAL; -static inline void set_timer_deinitalized(unsigned timer) -{ - once &= ~(1 << timer); + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; } static inline int channels_timer(unsigned channel) @@ -334,24 +343,6 @@ static uint32_t get_timer_channels(unsigned timer) return channels_cache[timer]; } -static inline int is_channels_timer_uninitalized(unsigned channel) -{ - return is_timer_uninitalized(channels_timer(channel)); -} - -int io_timer_is_channel_free(unsigned channel) -{ - int rv = io_timer_validate_channel_index(channel); - - if (rv == 0) { - if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) { - rv = -EBUSY; - } - } - - return rv; -} - int io_timer_validate_channel_index(unsigned channel) { int rv = -EINVAL; @@ -444,21 +435,26 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode return before ^ channels; } -static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { - int rv = io_timer_is_channel_free(channel); + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; - if (rv == 0) { + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; + ret = 0; } - return rv; + px4_leave_critical_section(flags); + + return ret; } -static inline int free_channel_resource(unsigned channel) +int io_timer_unallocate_channel(unsigned channel) { int mode = io_timer_get_channel_mode(channel); @@ -471,24 +467,6 @@ static inline int free_channel_resource(unsigned channel) return mode; } -int io_timer_free_channel(unsigned channel) -{ - if (io_timer_validate_channel_index(channel) != 0) { - return -EINVAL; - } - - int mode = io_timer_get_channel_mode(channel); - - if (mode > IOTimerChanMode_NotUsed) { - io_timer_set_enable(false, mode, 1 << channel); - free_channel_resource(channel); - - } - - return 0; -} - - static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { int rv = -EINVAL; @@ -497,7 +475,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) rv = io_timer_validate_channel_index(channel); if (rv == 0) { - rv = allocate_channel_resource(channel, mode); + rv = io_timer_allocate_channel(channel, mode); } } @@ -601,9 +579,9 @@ static inline void io_timer_set_PWM_mode(unsigned timer) rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; } -void io_timer_trigger(void) +void io_timer_trigger(unsigned channels_mask) { - int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot); + int oneshots = io_timer_get_mode_channels(IOTimerChanMode_OneShot) & channels_mask; if (oneshots != 0) { uint32_t action_cache[MAX_IO_TIMERS] = {0}; @@ -633,18 +611,20 @@ void io_timer_trigger(void) } } -int io_timer_init_timer(unsigned timer) +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) { - /* Do this only once per timer */ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } - int rv = is_timer_uninitalized(timer); + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); - if (rv == 0) { + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { irqstate_t flags = px4_enter_critical_section(); - set_timer_initalized(timer); - /* enable the timer clock before we try to talk to it */ modifyreg32(io_timers[timer].clock_register, 0, io_timers[timer].clock_bit); @@ -664,7 +644,12 @@ int io_timer_init_timer(unsigned timer) rCCER(timer) = 0; rDCR(timer) = 0; - if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) { + if ((io_timers[timer].base == STM32_TIM1_BASE) + || (io_timers[timer].base == STM32_TIM8_BASE) +#ifdef STM32_TIM15_BASE + || (io_timers[timer].base == STM32_TIM15_BASE) +#endif + ) { /* master output enable = on */ @@ -721,70 +706,53 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned timer, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the timer */ - - uint32_t channels = get_timer_channels(timer); - - /* Check that all channels are either in PWM, Oneshot or Dshot*/ - - if ((channels & (channel_allocations[IOTimerChanMode_Dshot] | - channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { - - /* Change only a timer that is owned by pwm or one shot */ - - /* Request to use OneShot ?*/ + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } - if (PWM_RATE_ONESHOT == rate) { + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ - /* Request to use OneShot - * - * We are here because ALL these channels were either PWM or Dshot - * Now they need to be Oneshot - */ + uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); - int changePWMOut = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_OneShot); - int changedChannels = changePWMOut | changeDshot; + /* Request to use OneShot ?*/ - /* Did the allocation change */ - if (changedChannels) { - io_timer_set_oneshot_mode(timer); - } + if (PWM_RATE_ONESHOT == rate) { - } else { + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - /* Request to use PWM - * - * We are here because ALL these channels were either Oneshot or Dshot - * Now they need to be PWM - */ + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } - int changeOneShot = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_PWMOut); + } else { - if (changeOneShot && changeDshot) { - io_timer_set_PWM_mode(timer); - } + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - timer_set_rate(timer, rate); + if (changed_channels) { + io_timer_set_PWM_mode(timer); } - rv = OK; + timer_set_rate(timer, rate); } - return rv; + return OK; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } uint32_t gpio = 0; uint32_t clearbits = CCMR_C1_RESET; @@ -810,7 +778,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, gpio = timer_io_channels[channel].gpio_in; break; -#if defined(BOARD_HAS_CAPTURE) +#if !defined(BOARD_HAS_NO_CAPTURE) case IOTimerChanMode_Capture: setbits = CCMR_C1_CAPTURE_INIT; @@ -826,27 +794,32 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, return -EINVAL; } + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config + + int previous_mode = io_timer_get_channel_mode(channel); int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); - /* Valid channel should now be reserved in new mode */ + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ - if (rv >= 0) { + rv = io_timer_init_timer(timer, mode); - /* Blindly try to initialize the timer - it will only do it once */ + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } - io_timer_init_timer(channels_timer(channel)); + /* Valid channel should now be reserved in new mode */ - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { px4_arch_configgpio(gpio); } - - unsigned timer = channels_timer(channel); - - /* configure the channel */ uint32_t shifts = timer_io_channels[channel].timer_channel - 1; @@ -883,16 +856,17 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, rvalue |= setbits; rCCER(timer) = rvalue; -#if !defined(BOARD_HAS_CAPTURE) +#if defined(BOARD_HAS_NO_CAPTURE) UNUSED(dier_setbits); #else channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; rDIER(timer) |= dier_setbits << shifts; #endif - px4_leave_critical_section(flags); } + px4_leave_critical_section(flags); + return rv; } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c index aa929e6e7e8a..d2b6d202ec71 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c @@ -82,7 +82,8 @@ int up_pwm_servo_init(uint32_t channel_mask) for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (current & (1 << channel)) { - io_timer_free_channel(channel); + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); current &= ~(1 << channel); } } @@ -90,29 +91,34 @@ int up_pwm_servo_init(uint32_t channel_mask) /* Now allocate the new set */ + int ret_val = OK; + int channels_init_mask = 0; + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (channel_mask & (1 << channel)) { - /* First free any that were not PWM mode before */ - - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - /* OneShot is set later, with the set_rate_group_update call. Init to PWM mode for now */ - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } -void up_pwm_servo_deinit(void) +void up_pwm_servo_deinit(uint32_t channel_mask) { /* disable the timers */ - up_pwm_servo_arm(false); + up_pwm_servo_arm(false, channel_mask); } int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) @@ -132,21 +138,12 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) } } - return io_timer_set_rate(group, rate); -} - -void up_pwm_update(void) -{ - io_timer_trigger(); + return io_timer_set_pwm_rate(group, rate); } -int up_pwm_servo_set_rate(unsigned rate) +void up_pwm_update(unsigned channels_mask) { - for (unsigned i = 0; i < MAX_IO_TIMERS; i++) { - up_pwm_servo_set_rate_group_update(i, rate); - } - - return 0; + io_timer_trigger(channels_mask); } uint32_t up_pwm_servo_get_rate_group(unsigned group) @@ -158,8 +155,8 @@ uint32_t up_pwm_servo_get_rate_group(unsigned group) } void -up_pwm_servo_arm(bool armed) +up_pwm_servo_arm(bool armed, uint32_t channel_mask) { - io_timer_set_enable(armed, IOTimerChanMode_OneShot, IO_TIMER_ALL_MODES_CHANNELS); - io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS); + io_timer_set_enable(armed, IOTimerChanMode_OneShot, channel_mask); + io_timer_set_enable(armed, IOTimerChanMode_PWMOut, channel_mask); } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c index bec12b5c9d98..d716a8a0221c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c @@ -66,23 +66,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value) int up_pwm_trigger_init(uint32_t channel_mask) { /* Init channels */ - for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { + int ret_val = OK; + int channels_init_mask = 0; - // First free any that were not trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { - io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } /* Enable the timers */ - up_pwm_trigger_arm(true); + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_trigger_deinit() diff --git a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp index d4b79f47466c..cfe55b1868ea 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp @@ -94,8 +94,6 @@ #define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) -extern int io_timer_init_timer(unsigned timer); - static void led_pwm_channel_init(unsigned channel); int led_pwm_servo_set(unsigned channel, uint8_t value); @@ -297,7 +295,12 @@ led_pwm_servo_init(void) /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { #if defined(BOARD_HAS_SHARED_PWM_TIMERS) - io_timer_init_timer(i); + int ret = io_timer_init_timer(i, IOTimerChanMode_LED); + + if (ret != 0) { + return ret; + } + #else led_pwm_timer_init_timer(i); #endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt index f20735002a35..e4627c30bbd7 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/spi/CMakeLists.txt @@ -35,3 +35,5 @@ px4_add_library(arch_spi spi.cpp ) target_compile_options(arch_spi PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) + +target_link_libraries (arch_spi PRIVATE drivers_board) # px4_spi_buses diff --git a/platforms/nuttx/src/px4/stm/stm32_common/srgbled_dma/srgbled_dma.cpp b/platforms/nuttx/src/px4/stm/stm32_common/srgbled_dma/srgbled_dma.cpp index 3d89d829a53a..24676ff60b5c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/srgbled_dma/srgbled_dma.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/srgbled_dma/srgbled_dma.cpp @@ -233,9 +233,9 @@ // Was the N complementary timer output was used #if defined(S_RGB_LED_CHANNELN) -# define CCER_CCnxE GTIM_CCER_CC1NE +# define CCER_CCnxE ATIM_CCER_CC1NE #else -# define CCER_CCnxE GTIM_CCER_CC1E +# define CCER_CCnxE ATIM_CCER_CC1E #endif #if S_RGB_LED_CHANNEL == 1 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/version/board_mcu_version.c b/platforms/nuttx/src/px4/stm/stm32_common/version/board_mcu_version.c index 99c115789d89..eac83754a61b 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/version/board_mcu_version.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/version/board_mcu_version.c @@ -99,6 +99,10 @@ int board_mcu_version(char *rev, const char **revstr, const char **errata) *revstr = "STM32F76xxx"; break; + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: *revstr = "STM32F42x"; /* Set possible errata */ diff --git a/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt index 65766e3d23d5..5659f4e5022e 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt @@ -36,3 +36,5 @@ add_subdirectory(../stm32_common/hrt hrt) add_subdirectory(../stm32_common/board_critmon board_critmon) add_subdirectory(../stm32_common/board_reset board_reset) add_subdirectory(../stm32_common/version version) + +add_subdirectory(watchdog) diff --git a/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt new file mode 100644 index 000000000000..3c80878d026d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/CMakeLists.txt @@ -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. +# +############################################################################ + +px4_add_library(arch_watchdog_iwdg + iwdg.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c new file mode 100644 index 000000000000..100b55cc568b --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/watchdog/iwdg.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#include + +#include "chip.h" +#include "stm32.h" +#include +#include "nvic.h" + +/**************************************************************************** + * Name: watchdog_pet() + * + * Description: + * This function resets the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_pet(void) +{ + putreg32(IWDG_KR_KEY_RELOAD, STM32_IWDG_KR); +} + +/**************************************************************************** + * Name: watchdog_init_ex() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * prescale - 0 - 7. + * reload - 0 - 0xfff. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_init_ex(int prescale, int reload) +{ + +#if defined(CONFIG_STM32_JTAG_FULL_ENABLE) || \ + defined(CONFIG_STM32_JTAG_NOJNTRST_ENABLE) || \ + defined(CONFIG_STM32_JTAG_SW_ENABLE) + putreg32(getreg32(STM32_DBGMCU_CR) | DBGMCU_CR_IWDGSTOP, STM32_DBGMCU_CR); +#endif + + /* unlock */ + + putreg32(IWDG_KR_KEY_ENABLE, STM32_IWDG_KR); + + /* Set the prescale value */ + + putreg32((prescale << IWDG_PR_SHIFT) & IWDG_PR_MASK, STM32_IWDG_PR); + + /* Set the reload value */ + + putreg32((reload << IWDG_RLR_RL_SHIFT) & IWDG_RLR_RL_MASK, STM32_IWDG_RLR); + + /* Start the watch dog */ + + putreg32(IWDG_KR_KEY_START, STM32_IWDG_KR); + + watchdog_pet(); + +} + + +/**************************************************************************** + * Name: watchdog_init() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + + +void watchdog_init(void) +{ + // max prescaler and max timeout ~26214.4 ms / 4 ~= 6.5 seconds + watchdog_init_ex(IWDG_PR_DIV256 >> IWDG_PR_SHIFT, IWDG_RLR_MAX >> IWDG_RLR_RL_SHIFT >> 2); +} diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/spi_hw_description.h index 4622f8cec5d3..2b9c3a7ecc0a 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/spi_hw_description.h @@ -34,6 +34,8 @@ #include "../../../stm32_common/include/px4_arch/spi_hw_description.h" +#if defined(CONFIG_SPI) + constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) { const bool nuttx_enabled_spi_buses[] = { @@ -84,3 +86,5 @@ constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX return false; } + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp index e2cf10277d8b..c86b67c73f97 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp @@ -36,6 +36,7 @@ * * Serial interface for PX4IO on STM32F4 */ +#include #include @@ -57,13 +58,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -98,7 +93,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -201,7 +195,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -232,6 +225,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { + // to be paranoid ensure all previous DMA transfers are cleared + _abort_dma(); + _current_packet = _packet; /* clear any lingering error status */ @@ -240,7 +236,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -287,8 +282,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -308,6 +301,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -318,6 +313,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break; diff --git a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt index 48a1aee444e0..b55e895db716 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt @@ -45,4 +45,4 @@ add_subdirectory(../stm32_common/tone_alarm tone_alarm) add_subdirectory(../stm32_common/version version) add_subdirectory(px4io_serial) - +add_subdirectory(watchdog) diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h index 4652afe3497d..540c09a64620 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h @@ -39,11 +39,15 @@ __BEGIN_DECLS #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F7 #include +#include +#include #include #include //include up_systemreset() which is included on stm32.h -#include -#define PX4_BBSRAM_SIZE STM32F7_BBSRAM_SIZE -#define PX4_BBSRAM_GETDESC_IOCTL STM32F7_BBSRAM_GETDESC_IOCTL +#if defined(CONFIG_STM32F7_BKPSRAM) +# include +# define PX4_BBSRAM_SIZE STM32F7_BBSRAM_SIZE +# define PX4_BBSRAM_GETDESC_IOCTL STM32F7_BBSRAM_GETDESC_IOCTL +#endif // CONFIG_STM32F7_BKPSRAM #define PX4_FLASH_BASE 0x08000000 #define PX4_NUMBER_I2C_BUSES STM32F7_NI2C #define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 18 diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h index 0da37cab9ec6..702635fdb448 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h @@ -32,6 +32,8 @@ ****************************************************************************/ #pragma once +#if defined(CONFIG_SPI) + #include "../../../stm32_common/include/px4_arch/spi_hw_description.h" constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) @@ -84,3 +86,5 @@ constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX return false; } + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp index 0161736a7ba3..a640166e0540 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp @@ -37,6 +37,8 @@ * Serial interface for PX4IO on STM32F7 */ +#include + #include #include "stm32_uart.h" @@ -66,13 +68,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -107,7 +103,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -212,7 +207,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -243,6 +237,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { + // to be paranoid ensure all previous DMA transfers are cleared + _abort_dma(); + _current_packet = _packet; /* clear data that may be in the RDR and clear overrun error: */ @@ -254,7 +251,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -305,8 +301,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 &= ~USART_CR1_TE; //rCR1 |= USART_CR1_TE; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -326,6 +320,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -336,6 +332,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break; diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt new file mode 100644 index 000000000000..3c80878d026d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/CMakeLists.txt @@ -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. +# +############################################################################ + +px4_add_library(arch_watchdog_iwdg + iwdg.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c new file mode 100644 index 000000000000..a2c272a38549 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/iwdg.c @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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. + * + ****************************************************************************/ + +#include +#include "arm_internal.h" +#include "arm_arch.h" +#include "chip.h" + +#include "nvic.h" + +#include "stm32_wdg.h" + +/**************************************************************************** + * Name: watchdog_pet() + * + * Description: + * This function resets the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_pet(void) +{ + putreg32(IWDG_KR_KEY_RELOAD, STM32_IWDG_KR); +} + +/**************************************************************************** + * Name: watchdog_init() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_init(void) +{ +#if defined(CONFIG_STM32F7_JTAG_FULL_ENABLE) || \ + defined(CONFIG_STM32F7_JTAG_NOJNTRST_ENABLE) || \ + defined(CONFIG_STM32F7_JTAG_SW_ENABLE) + putreg32(getreg32(STM32_DBGMCU_APB1_FZ) | DBGMCU_APB1_IWDGSTOP, STM32_DBGMCU_APB1_FZ); +#endif + + /* unlock */ + + putreg32(IWDG_KR_KEY_ENABLE, STM32_IWDG_KR); + + /* Set the prescale value */ + + putreg32(IWDG_PR_DIV16, STM32_IWDG_PR); + + /* Set the reload value */ + + putreg32(IWDG_RLR_MAX, STM32_IWDG_RLR); + + /* Start the watch dog */ + + putreg32(IWDG_KR_KEY_START, STM32_IWDG_KR); + + watchdog_pet(); +} diff --git a/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h new file mode 100644 index 000000000000..9150b841bed0 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/watchdog/stm32_wdg.h @@ -0,0 +1,162 @@ +/************************************************************************************ + * arch/arm/src/stm32/hardware/stm32_wdg.h + * + * Copyright (C) 2009, 2011-2013 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. + * + ************************************************************************************/ + +#ifndef __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H +#define __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include "chip.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Register Offsets *****************************************************************/ + +#define STM32_IWDG_KR_OFFSET 0x0000 /* Key register (32-bit) */ +#define STM32_IWDG_PR_OFFSET 0x0004 /* Prescaler register (32-bit) */ +#define STM32_IWDG_RLR_OFFSET 0x0008 /* Reload register (32-bit) */ +#define STM32_IWDG_SR_OFFSET 0x000c /* Status register (32-bit) */ +#if defined(CONFIG_STM32_STM32F30XX) +# define STM32_IWDG_WINR_OFFSET 0x000c /* Window register (32-bit) */ +#endif + +#define STM32_WWDG_CR_OFFSET 0x0000 /* Control Register (32-bit) */ +#define STM32_WWDG_CFR_OFFSET 0x0004 /* Configuration register (32-bit) */ +#define STM32_WWDG_SR_OFFSET 0x0008 /* Status register (32-bit) */ + +/* Register Addresses ***************************************************************/ + +#define STM32_IWDG_KR (STM32_IWDG_BASE+STM32_IWDG_KR_OFFSET) +#define STM32_IWDG_PR (STM32_IWDG_BASE+STM32_IWDG_PR_OFFSET) +#define STM32_IWDG_RLR (STM32_IWDG_BASE+STM32_IWDG_RLR_OFFSET) +#define STM32_IWDG_SR (STM32_IWDG_BASE+STM32_IWDG_SR_OFFSET) +#if defined(CONFIG_STM32_STM32F30XX) +# define STM32_IWDG_WINR (STM32_IWDG_BASE+STM32_IWDG_WINR_OFFSET) +#endif + +#define STM32_WWDG_CR (STM32_WWDG_BASE+STM32_WWDG_CR_OFFSET) +#define STM32_WWDG_CFR (STM32_WWDG_BASE+STM32_WWDG_CFR_OFFSET) +#define STM32_WWDG_SR (STM32_WWDG_BASE+STM32_WWDG_SR_OFFSET) + +/* Register Bitfield Definitions ****************************************************/ + +/* Key register (32-bit) */ + +#define IWDG_KR_KEY_SHIFT (0) /* Bits 15-0: Key value (write only, read 0000h) */ +#define IWDG_KR_KEY_MASK (0xffff << IWDG_KR_KEY_SHIFT) + +#define IWDG_KR_KEY_ENABLE (0x5555) /* Enable register access */ +#define IWDG_KR_KEY_DISABLE (0x0000) /* Disable register access */ +#define IWDG_KR_KEY_RELOAD (0xaaaa) /* Reload the counter */ +#define IWDG_KR_KEY_START (0xcccc) /* Start the watchdog */ + +/* Prescaler register (32-bit) */ + +#define IWDG_PR_SHIFT (0) /* Bits 2-0: Prescaler divider */ +#define IWDG_PR_MASK (7 << IWDG_PR_SHIFT) +# define IWDG_PR_DIV4 (0 << IWDG_PR_SHIFT) /* 000: divider /4 */ +# define IWDG_PR_DIV8 (1 << IWDG_PR_SHIFT) /* 001: divider /8 */ +# define IWDG_PR_DIV16 (2 << IWDG_PR_SHIFT) /* 010: divider /16 */ +# define IWDG_PR_DIV32 (3 << IWDG_PR_SHIFT) /* 011: divider /32 */ +# define IWDG_PR_DIV64 (4 << IWDG_PR_SHIFT) /* 100: divider /64 */ +# define IWDG_PR_DIV128 (5 << IWDG_PR_SHIFT) /* 101: divider /128 */ +# define IWDG_PR_DIV256 (6 << IWDG_PR_SHIFT) /* 11x: divider /256 */ + +/* Reload register (32-bit) */ + +#define IWDG_RLR_RL_SHIFT (0) /* Bits11:0 RL[11:0]: Watchdog counter reload value */ +#define IWDG_RLR_RL_MASK (0x0fff << IWDG_RLR_RL_SHIFT) + +#define IWDG_RLR_MAX (0xfff) + +/* Status register (32-bit) */ + +#define IWDG_SR_PVU (1 << 0) /* Bit 0: Watchdog prescaler value update */ +#define IWDG_SR_RVU (1 << 1) /* Bit 1: Watchdog counter reload value update */ + +#if defined(CONFIG_STM32_STM32F30XX) +# define IWDG_SR_WVU (1 << 2) /* Bit 2: */ +#endif + +/* Window register (32-bit) */ + +#if defined(CONFIG_STM32_STM32F30XX) +# define IWDG_WINR_SHIFT (0) +# define IWDG_WINR_MASK (0x0fff << IWDG_WINR_SHIFT) +#endif + +/* Control Register (32-bit) */ + +#define WWDG_CR_T_SHIFT (0) /* Bits 6:0 T[6:0]: 7-bit counter (MSB to LSB) */ +#define WWDG_CR_T_MASK (0x7f << WWDG_CR_T_SHIFT) +# define WWDG_CR_T_MAX (0x3f << WWDG_CR_T_SHIFT) +# define WWDG_CR_T_RESET (0x40 << WWDG_CR_T_SHIFT) +#define WWDG_CR_WDGA (1 << 7) /* Bit 7: Activation bit */ + +/* Configuration register (32-bit) */ + +#define WWDG_CFR_W_SHIFT (0) /* Bits 6:0 W[6:0] 7-bit window value */ +#define WWDG_CFR_W_MASK (0x7f << WWDG_CFR_W_SHIFT) +#define WWDG_CFR_WDGTB_SHIFT (7) /* Bits 8:7 [1:0]: Timer Base */ +#define WWDG_CFR_WDGTB_MASK (3 << WWDG_CFR_WDGTB_SHIFT) +# define WWDG_CFR_PCLK1 (0 << WWDG_CFR_WDGTB_SHIFT) /* 00: CK Counter Clock (PCLK1 div 4096) div 1 */ +# define WWDG_CFR_PCLK1d2 (1 << WWDG_CFR_WDGTB_SHIFT) /* 01: CK Counter Clock (PCLK1 div 4096) div 2 */ +# define WWDG_CFR_PCLK1d4 (2 << WWDG_CFR_WDGTB_SHIFT) /* 10: CK Counter Clock (PCLK1 div 4096) div 4 */ +# define WWDG_CFR_PCLK1d8 (3 << WWDG_CFR_WDGTB_SHIFT) /* 11: CK Counter Clock (PCLK1 div 4096) div 8 */ +#define WWDG_CFR_EWI (1 << 9) /* Bit 9: Early Wakeup Interrupt */ + +/* Status register (32-bit) */ + +#define WWDG_SR_EWIF (1 << 0) /* Bit 0: Early Wakeup Interrupt Flag */ + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#endif /* __ARCH_ARM_SRC_STM32_HARDWARE_STM32_WDG_H */ diff --git a/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp b/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp index 68d640fa5594..80f98c564fd1 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp +++ b/platforms/nuttx/src/px4/stm/stm32h7/adc/adc.cpp @@ -62,7 +62,7 @@ #define rPCSEL(base) REG((base), STM32_ADC_PCSEL_OFFSET) #define rCFG(base) REG((base), STM32_ADC_CFGR_OFFSET) #define rCFG2(base) REG((base), STM32_ADC_CFGR2_OFFSET) -#define rCCR() REG((STM32_ADC1_BASE), (STM32_ADC_CCR_OFFSET)) +#define rCCR(base) REG((base), STM32_ADC_CCR_OFFSET) // Offset has ADC CMN included #define rSQR1(base) REG((base), STM32_ADC_SQR1_OFFSET) #define rSQR2(base) REG((base), STM32_ADC_SQR2_OFFSET) #define rSQR3(base) REG((base), STM32_ADC_SQR3_OFFSET) @@ -168,8 +168,8 @@ int px4_arch_adc_init(uint32_t base_address) /* enable the temperature sensor, VREFINT channel and VBAT */ - rCCR() = (ADC_CCR_VREFEN | ADC_CCR_VSENSEEN | ADC_CCR_VBATEN | - ADC_CCR_CKMODE_ASYCH | ADC_CCR_PRESC_DIV); + rCCR(base_address) = (ADC_CCR_VREFEN | ADC_CCR_VSENSEEN | ADC_CCR_VBATEN | + ADC_CCR_CKMODE_ASYCH | ADC_CCR_PRESC_DIV); /* Enable ADC calibration. ADCALDIF == 0 so this is only for * single-ended conversions, not for differential ones. diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index c2ae6c9ce7fc..a93b9538bee6 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -85,6 +85,9 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const case Timer::Timer12: case Timer::Timer13: case Timer::Timer14: + case Timer::Timer15: + case Timer::Timer16: + case Timer::Timer17: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h index 1bdbb296c4db..b1299b04e336 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h @@ -39,6 +39,8 @@ #include #include +#define initIOTimerChannelCapture initIOTimerChannel // alias, used for param metadata generation + static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerChannel timer, GPIO::GPIOPin pin) { timer_io_channels_t ret{}; @@ -47,6 +49,8 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha switch (timer.timer) { case Timer::Timer1: case Timer::Timer2: + case Timer::Timer16: + case Timer::Timer17: gpio_af = GPIO_AF1; break; @@ -66,6 +70,10 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha gpio_af = GPIO_AF3; break; + case Timer::Timer15: + gpio_af = GPIO_AF4; + break; + case Timer::Timer13: case Timer::Timer14: gpio_af = GPIO_AF9; @@ -257,6 +265,39 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm ret.vectorno = STM32_IRQ_TIM14; #ifdef CONFIG_STM32_TIM14 nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer15: + ret.base = STM32_TIM15_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM15EN; + ret.clock_freq = STM32_APB2_TIM15_CLKIN; + ret.vectorno = STM32_IRQ_TIM15; +#ifdef CONFIG_STM32_TIM15 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer16: + ret.base = STM32_TIM16_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM16EN; + ret.clock_freq = STM32_APB2_TIM16_CLKIN; + ret.vectorno = STM32_IRQ_TIM16; +#ifdef CONFIG_STM32_TIM16 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer17: + ret.base = STM32_TIM17_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM17EN; + ret.clock_freq = STM32_APB2_TIM17_CLKIN; + ret.vectorno = STM32_IRQ_TIM17; +#ifdef CONFIG_STM32_TIM17 + nuttx_config_timer_enabled = true; #endif break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/micro_hal.h index 31559c7bf864..93e80aaa491b 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/micro_hal.h @@ -44,6 +44,7 @@ __BEGIN_DECLS #define STM32_RCC_APB1RSTR STM32_RCC_APB1LRSTR #define RCC_APB1ENR_TIM2EN RCC_APB1LENR_TIM2EN #define RCC_APB1ENR_TIM3EN RCC_APB1LENR_TIM3EN +#define RCC_APB1ENR_TIM4EN RCC_APB1LENR_TIM4EN #define RCC_APB1ENR_TIM5EN RCC_APB1LENR_TIM5EN #define RCC_APB1ENR_TIM14EN RCC_APB1LENR_TIM14EN #define RCC_APB1RSTR_TIM2RST RCC_APB1LRSTR_TIM2RST diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/spi_hw_description.h index 90aeee1c5c44..e545b2273b34 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/spi_hw_description.h @@ -34,6 +34,8 @@ #include "../../../stm32_common/include/px4_arch/spi_hw_description.h" +#if defined(CONFIG_SPI) + constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) { const bool nuttx_enabled_spi_buses[] = { @@ -84,3 +86,5 @@ constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX return false; } + +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp index 5e35ab1ddae7..1492a7fc19af 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp @@ -37,8 +37,9 @@ * Serial interface for PX4IO on STM32F7 */ -#include +#include +#include #include "stm32_uart.h" #include @@ -105,13 +106,7 @@ ArchPX4IOSerial::ArchPX4IOSerial() : _current_packet(nullptr), _rx_dma_status(_dma_status_inactive), _completion_semaphore(SEM_INITIALIZER(0)), -#if 0 - _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), - _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")) -#else - _pc_dmasetup(nullptr), - _pc_dmaerrs(nullptr) -#endif + _pc_dmaerrs(perf_alloc(PC_COUNT, MODULE_NAME": DMA errors")) { } @@ -146,7 +141,6 @@ ArchPX4IOSerial::~ArchPX4IOSerial() /* and kill our semaphores */ px4_sem_destroy(&_completion_semaphore); - perf_free(_pc_dmasetup); perf_free(_pc_dmaerrs); } @@ -251,7 +245,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) if (count >= 5000) { syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); perf_print_counter(_pc_txns); - perf_print_counter(_pc_dmasetup); perf_print_counter(_pc_retries); perf_print_counter(_pc_timeouts); perf_print_counter(_pc_crcerrs); @@ -282,6 +275,9 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) int ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { + // to be paranoid ensure all previous DMA transfers are cleared + _abort_dma(); + _current_packet = _packet; /* clear data that may be in the RDR and clear overrun error: */ @@ -293,7 +289,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) /* start RX DMA */ perf_begin(_pc_txns); - perf_begin(_pc_dmasetup); /* DMA setup time ~3µs */ _rx_dma_status = _dma_status_waiting; @@ -317,7 +312,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) DMA_SCR_PSIZE_8BITS | DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); + DMA_SCR_MBURST_SINGLE | + DMA_SCR_TRBUFF); rxdmacfg.cfg2 = 0; @@ -342,7 +338,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) DMA_SCR_PSIZE_8BITS | DMA_SCR_MSIZE_8BITS | DMA_SCR_PBURST_SINGLE | - DMA_SCR_MBURST_SINGLE); + DMA_SCR_MBURST_SINGLE | + DMA_SCR_TRBUFF); txdmacfg.cfg2 = 0; stm32_dmasetup(_tx_dma, &txdmacfg); @@ -351,8 +348,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) //rCR1 &= ~USART_CR1_TE; //rCR1 |= USART_CR1_TE; - perf_end(_pc_dmasetup); - /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); @@ -372,6 +367,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) if (ret == OK) { /* check for DMA errors */ if (_rx_dma_status & DMA_STATUS_TEIF) { + // stream transfer error, ensure all DMA is also stopped before exiting early + _abort_dma(); perf_count(_pc_dmaerrs); ret = -EIO; break; @@ -382,6 +379,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) _current_packet->crc = 0; if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + _abort_dma(); perf_count(_pc_crcerrs); ret = -EIO; break; diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 927c3e35b2ae..2cae2595ac6c 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -25,73 +25,58 @@ px4_posix_generate_alias( PREFIX ${PX4_SHELL_COMMAND_PREFIX} ) -if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlflight_excelsior")) - include(fastrpc) - include(linux_app) - - FASTRPC_STUB_GEN(../qurt/px4muorb.idl) - - LINUX_APP( - APP_NAME px4 - IDL_NAME px4muorb - APPS_DEST "/home/linaro" - SOURCES - px4muorb_stub.c - src/px4/common/main.cpp - apps.cpp - LINK_LIBS - -Wl,--start-group - px4_layer - parameters - ${module_libraries} - ${FASTRPC_ARM_LIBS} - pthread m rt - -Wl,--end-group +if (CMAKE_BUILD_TYPE STREQUAL FuzzTesting) + add_executable(px4 + src/px4/common/main_fuzztesting.cpp + apps.cpp ) - else() add_executable(px4 src/px4/common/main.cpp apps.cpp ) +endif() - target_link_libraries(px4 - PRIVATE - ${module_libraries} - pthread m - - # horrible circular dependencies that need to be teased apart - px4_layer px4_platform - work_queue - parameters - ) - - if(NOT APPLE) - target_link_libraries(px4 PRIVATE rt) - endif() - - target_link_libraries(px4 PRIVATE uORB) +target_link_libraries(px4 + PRIVATE + ${module_libraries} + m + parameters +) - #============================================================================= - # install - # +if((NOT APPLE) AND (NOT ANDROID)) + target_link_libraries(px4 PRIVATE rt) +endif() - # TODO: extend to snapdragon +if(NOT ANDROID) + target_link_libraries(px4 PRIVATE pthread) +endif() - # px4 dirs - install( - DIRECTORY - ${PROJECT_SOURCE_DIR}/posix-configs - ${PROJECT_SOURCE_DIR}/test - ${CMAKE_BINARY_DIR}/etc - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} - DESTINATION - ${PROJECT_NAME} - USE_SOURCE_PERMISSIONS - ) +target_link_libraries(px4 PRIVATE uORB) +if (CMAKE_BUILD_TYPE STREQUAL FuzzTesting) + target_include_directories(px4 PRIVATE SYSTEM "${CMAKE_BINARY_DIR}/mavlink}") + target_compile_options(px4 PRIVATE "-Wno-cast-align") endif() +#============================================================================= +# install +# + +# TODO: extend to snapdragon + +# px4 dirs +install( + DIRECTORY + ${PROJECT_SOURCE_DIR}/posix-configs + ${PROJECT_SOURCE_DIR}/test + ${CMAKE_BINARY_DIR}/etc + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} + DESTINATION + ${PROJECT_NAME} + USE_SOURCE_PERMISSIONS +) + # Module Symlinks px4_posix_generate_symlinks( MODULE_LIST ${module_libraries} diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake index 3d9bada3c1c3..8137c75d1906 100644 --- a/platforms/posix/cmake/px4_impl_os.cmake +++ b/platforms/posix/cmake/px4_impl_os.cmake @@ -301,6 +301,7 @@ function(px4_os_prebuild_targets) ARGN ${ARGN}) add_library(prebuild_targets INTERFACE) + target_link_libraries(prebuild_targets INTERFACE px4_layer drivers_board) add_dependencies(prebuild_targets DEPENDS uorb_headers) endfunction() diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 0257cbd344f4..a71e3efbcf56 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -9,7 +9,6 @@ add_custom_command(OUTPUT ${PX4_BINARY_DIR}/logs ) add_custom_target(logs_symlink DEPENDS ${PX4_BINARY_DIR}/logs) add_dependencies(px4 logs_symlink) - add_custom_target(run_config COMMAND Tools/sitl_run.sh $ ${config_sitl_debugger} ${config_sitl_viewer} ${config_sitl_model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR} WORKING_DIRECTORY ${SITL_WORKING_DIR} @@ -21,15 +20,41 @@ px4_add_git_submodule(TARGET git_jmavsim PATH "${PX4_SOURCE_DIR}/Tools/jMAVSim") # Add support for external project building include(ExternalProject) -include(ProcessorCount) -set(build_cores 1) +# Estimate an appropriate number of parallel jobs +cmake_host_system_information(RESULT AVAILABLE_PHYSICAL_MEMORY QUERY AVAILABLE_PHYSICAL_MEMORY) +cmake_host_system_information(RESULT NUMBER_OF_LOGICAL_CORES QUERY NUMBER_OF_LOGICAL_CORES) + +set(parallel_jobs 1) -ProcessorCount(N) -if(N GREATER_EQUAL 4) - math(EXPR build_cores "${N} - 2") +if(NOT NUMBER_OF_LOGICAL_CORES) + include(ProcessorCount) + ProcessorCount(NUMBER_OF_LOGICAL_CORES) endif() +if(NOT AVAILABLE_PHYSICAL_MEMORY AND NUMBER_OF_LOGICAL_CORES GREATER_EQUAL 4) + # Memory estimate unavailable, use N-2 jobs + math(EXPR parallel_jobs "${NUMBER_OF_LOGICAL_CORES} - 2") +endif() + +if (AVAILABLE_PHYSICAL_MEMORY) + # Allow an additional job for every 1.5GB of available physical memory + math(EXPR parallel_jobs "${AVAILABLE_PHYSICAL_MEMORY}/(3*1024/2)") +else() + set(AVAILABLE_PHYSICAL_MEMORY "?") +endif() + +if(parallel_jobs GREATER NUMBER_OF_LOGICAL_CORES) + set(parallel_jobs ${NUMBER_OF_LOGICAL_CORES}) +endif() + +if(parallel_jobs LESS 1) + set(parallel_jobs 1) +endif() + +message(DEBUG "${NUMBER_OF_LOGICAL_CORES} logical cores detected and ${AVAILABLE_PHYSICAL_MEMORY} megabytes of memory available. + Limiting sitl_gazebo and simulation-ignition concurrent jobs to ${parallel_jobs}") + # project to build sitl_gazebo if necessary px4_add_git_submodule(TARGET git_gazebo PATH "${PX4_SOURCE_DIR}/Tools/sitl_gazebo") ExternalProject_Add(sitl_gazebo @@ -45,7 +70,21 @@ ExternalProject_Add(sitl_gazebo USES_TERMINAL_BUILD true EXCLUDE_FROM_ALL true BUILD_ALWAYS 1 - BUILD_COMMAND ${CMAKE_COMMAND} --build -- -j ${build_cores} + BUILD_COMMAND ${CMAKE_COMMAND} --build -- -j ${parallel_jobs} +) + +px4_add_git_submodule(TARGET git_ign_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation-ignition") +ExternalProject_Add(simulation-ignition + SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation-ignition + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + BINARY_DIR ${PX4_BINARY_DIR}/build_ign_gazebo + INSTALL_COMMAND "" + DEPENDS git_ign_gazebo + USES_TERMINAL_CONFIGURE true + USES_TERMINAL_BUILD true + EXCLUDE_FROM_ALL true + BUILD_ALWAYS 1 + BUILD_COMMAND ${CMAKE_COMMAND} --build -- -j ${parallel_jobs} ) ExternalProject_Add(mavsdk_tests @@ -90,6 +129,7 @@ set(viewers none jmavsim gazebo + ignition ) set(debuggers @@ -102,8 +142,10 @@ set(debuggers set(models none + believer boat cloudship + glider if750a iris iris_ctrlalloc @@ -114,13 +156,14 @@ set(models iris_opt_flow iris_opt_flow_mockup iris_rplidar - iris_rtps px4vision iris_vision nxp_cupcar + omnicopter plane plane_cam plane_catapult plane_lidar + px4vision r1_rover rover shell @@ -131,6 +174,7 @@ set(models techpod tiltrotor typhoon_h480 + typhoon_h480_ctrlalloc uuv_bluerov2_heavy uuv_hippocampus ) @@ -178,6 +222,8 @@ foreach(viewer ${viewers}) add_dependencies(${_targ_name} px4 sitl_gazebo) elseif(viewer STREQUAL "jmavsim") add_dependencies(${_targ_name} px4 git_jmavsim) + elseif(viewer STREQUAL "ignition") + add_dependencies(${_targ_name} px4 simulation-ignition) endif() else() if(viewer STREQUAL "gazebo") @@ -278,6 +324,70 @@ foreach(debugger ${debuggers}) endforeach() endforeach() +# create targets for sihsim +set(models_sih + none + airplane + quadx + xvert +) + +set(worlds_sih + none +) + +foreach(debugger ${debuggers}) + foreach(model ${models_sih}) + foreach(world ${worlds_sih}) + if(world STREQUAL "none") + if(debugger STREQUAL "none") + if(model STREQUAL "none") + set(_targ_name "sihsim") + else() + set(_targ_name "sihsim_${model}") + endif() + else() + if(model STREQUAL "none") + set(_targ_name "sihsim__${debugger}_${world}") + else() + set(_targ_name "sihsim_${model}_${debugger}_${world}") + endif() + endif() + + add_custom_target(${_targ_name} + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $ ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR} + WORKING_DIRECTORY ${SITL_WORKING_DIR} + USES_TERMINAL + DEPENDS logs_symlink + ) + list(APPEND all_posix_vmd_make_targets ${_targ_name}) + else() + if(debugger STREQUAL "none") + if(model STREQUAL "none") + set(_targ_name "sihsim___${world}") + else() + set(_targ_name "sihsim_${model}__${world}") + endif() + else() + if(model STREQUAL "none") + set(_targ_name "sihsim___${debugger}_${world}") + else() + set(_targ_name "sihsim_${model}_${debugger}_${world}") + endif() + endif() + + add_custom_target(${_targ_name} + COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $ ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR} + WORKING_DIRECTORY ${SITL_WORKING_DIR} + USES_TERMINAL + DEPENDS logs_symlink + ) + list(APPEND all_posix_vmd_make_targets ${_targ_name}) + endif() + endforeach() + endforeach() +endforeach() + # add flighgear targets if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no") set(models diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index ddf6f303621c..44194d0b0d2d 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -8,7 +8,6 @@ set(tests bezier bitset bson - conv dataman file2 float @@ -19,17 +18,11 @@ set(tests List mathlib matrix - microbench_atomic - microbench_hrt - microbench_math - microbench_matrix - microbench_uorb mixer param parameters perf search_min - servo sleep versioning ) @@ -96,6 +89,20 @@ set_tests_properties(sitl-mavlink PROPERTIES PASS_REGULAR_EXPRESSION "ALL TESTS sanitizer_fail_test_on_error(sitl-mavlink) +# IMU filtering +add_test(NAME sitl-imu_filtering + COMMAND $ + -s ${PX4_SOURCE_DIR}/posix-configs/SITL/init/test/test_imu_filtering + -t ${PX4_SOURCE_DIR}/test_data + ${PX4_SOURCE_DIR}/ROMFS/px4fmu_test + WORKING_DIRECTORY ${SITL_WORKING_DIR} +) + +set_tests_properties(sitl-imu_filtering PROPERTIES FAIL_REGULAR_EXPRESSION "FAIL") +set_tests_properties(sitl-imu_filtering PROPERTIES PASS_REGULAR_EXPRESSION "ALL TESTS PASSED") +sanitizer_fail_test_on_error(sitl-imu_filtering) + + # # Shutdown test # add_test(NAME sitl-shutdown diff --git a/platforms/posix/include/system_config.h b/platforms/posix/include/system_config.h index 46abd18a035c..7d3162cbe0ce 100644 --- a/platforms/posix/include/system_config.h +++ b/platforms/posix/include/system_config.h @@ -63,4 +63,3 @@ #define CONFIG_SCHED_WORKPERIOD 50000 #define CONFIG_SCHED_INSTRUMENTATION 1 -#define CONFIG_MAX_TASKS 32 diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 48656a242030..34b65cdf44aa 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -36,17 +36,6 @@ add_subdirectory(px4_daemon) add_subdirectory(lockstep_scheduler) set(EXTRA_DEPENDS) -if("${CONFIG_SHMEM}" STREQUAL "1") - list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") - include(hexagon_sdk) - include_directories(${HEXAGON_SDK_INCLUDES}) - include_directories(${PX4_BINARY_DIR}/platforms/posix) - list(APPEND SHMEM_SRCS - shmem_posix.cpp - ) - add_definitions(-DCONFIG_SHMEM=1) - set(EXTRA_DEPENDS generate_px4muorb_stubs) -endif() add_library(px4_layer px4_posix_impl.cpp @@ -57,7 +46,6 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp - ${SHMEM_SRCS} ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index fa932933893d..6253e8171e3a 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -95,7 +95,8 @@ hrt_abstime hrt_absolute_time_offset() static void hrt_lock() { - px4_sem_wait(&_hrt_lock); + // loop as the wait may be interrupted by a signal + do {} while (px4_sem_wait(&_hrt_lock) != 0); } static void hrt_unlock() @@ -144,33 +145,6 @@ hrt_abstime hrt_absolute_time() #endif // defined(ENABLE_LOCKSTEP_SCHEDULER) } -/* - * Convert a timespec to absolute time. - */ -hrt_abstime ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/* - * Compute the delta between a timestamp taken in the past - * and now. - * - * This function is safe to use even if the timestamp is updated - * by an interrupt during execution. - */ -hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - // This is not atomic as the value on the application layer of POSIX is limited. - hrt_abstime delta = hrt_absolute_time() - *then; - return delta; -} - /* * Store the absolute time in an interrupt-safe fashion. * @@ -360,7 +334,7 @@ hrt_call_reschedule() // Remove the existing expiry and update with the new expiry hrt_work_cancel(&_hrt_work); - hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay); + hrt_work_queue(&_hrt_work, &hrt_tim_isr, nullptr, delay); } static void @@ -498,13 +472,6 @@ hrt_call_invoke() hrt_unlock(); } -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) { if (clk_id == CLOCK_MONOTONIC) { diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp index 2875ae298f6d..5492f72b28dd 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp +++ b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp @@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component) int components_used_bitset = _components_used_bitset; - if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) { + if (_components_progress_bitset == components_used_bitset) { _components_progress_bitset = 0; px4_sem_post(&_components_sem); } diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index e8c29bb3e734..901ffdd72212 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -317,15 +317,6 @@ int main(int argc, char **argv) pxh.run_pxh(); } - // When we exit, we need to stop muorb on Snapdragon. - -#ifdef __PX4_POSIX_EAGLE - // Sending muorb stop is needed if it is running to exit cleanly. - // TODO: we should check with px4_task_is_running("muorb") before stopping it. - std::string muorb_stop_cmd("muorb stop"); - px4_daemon::Pxh::process_line(muorb_stop_cmd, true); -#endif - std::string cmd("shutdown"); px4_daemon::Pxh::process_line(cmd, true); @@ -371,7 +362,7 @@ int create_symlinks_if_needed(std::string &data_path) } - PX4_INFO("Creating symlink %s -> %s", src_path.c_str(), dest_path.c_str()); + PX4_INFO_RAW("Creating symlink %s -> %s\n", src_path.c_str(), dest_path.c_str()); // create sym-link int ret = symlink(src_path.c_str(), dest_path.c_str()); @@ -451,10 +442,7 @@ void sig_int_handler(int sig_num) void set_cpu_scaling() { -#ifdef __PX4_POSIX_EAGLE - // On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run - // at the maximum frequency all the time. - // Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon. +#if 0 system("echo performance > /sys/devices/system/cpu/cpu0/cpufreq/scaling_governor"); system("echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor"); diff --git a/platforms/posix/src/px4/common/main_fuzztesting.cpp b/platforms/posix/src/px4/common/main_fuzztesting.cpp new file mode 100644 index 000000000000..2a8588a0932b --- /dev/null +++ b/platforms/posix/src/px4/common/main_fuzztesting.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * This is an alternative main entrypoint for fuzz testing. + */ + +#include + +#include "px4_platform_common/init.h" +#include "px4_platform_common/posix.h" +#include "apps.h" +#include "px4_daemon/client.h" +#include "px4_daemon/server.h" +#include "px4_daemon/pxh.h" + +#include +#include +#include "common/mavlink.h" + +#define MODULE_NAME "px4" + +#ifndef PATH_MAX +#define PATH_MAX 1024 +#endif + + +namespace px4 +{ +void init_once(); +} + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, const size_t size); +void initialize_fake_px4_once(); +void send_mavlink(const uint8_t *data, const size_t size); + + +extern "C" int LLVMFuzzerTestOneInput(const uint8_t *data, const size_t size) +{ + initialize_fake_px4_once(); + + send_mavlink(data, size); + + return 0; +} + +void initialize_fake_px4_once() +{ + static bool first_time = true; + + if (!first_time) { + return; + } + + first_time = false; + + px4::init_once(); + px4::init(0, nullptr, "px4"); + + px4_daemon::Pxh pxh; + pxh.process_line("uorb start", true); + pxh.process_line("param load", true); + pxh.process_line("dataman start", true); + pxh.process_line("load_mon start", true); + pxh.process_line("battery_simulator start", true); + pxh.process_line("tone_alarm start", true); + pxh.process_line("rc_update start", true); + pxh.process_line("sensors start", true); + pxh.process_line("commander start", true); + pxh.process_line("navigator start", true); + pxh.process_line("ekf2 start", true); + pxh.process_line("mc_att_control start", true); + pxh.process_line("mc_pos_control start", true); + pxh.process_line("land_detector start multicopter", true); + pxh.process_line("logger start", true); + pxh.process_line("mavlink start -x -o 14540 -r 4000000", true); + pxh.process_line("mavlink boot_complete", true); +} + +void send_mavlink(const uint8_t *data, const size_t size) +{ + int socket_fd = socket(AF_INET, SOCK_DGRAM, 0); + + if (socket_fd < 0) { + PX4_ERR("socket error: %s", strerror(errno)); + return; + } + + struct sockaddr_in addr {}; + + addr.sin_family = AF_INET; + + inet_pton(AF_INET, "0.0.0.0", &(addr.sin_addr)); + + addr.sin_port = htons(14540); + + if (bind(socket_fd, reinterpret_cast(&addr), sizeof(addr)) != 0) { + PX4_ERR("bind error: %s", strerror(errno)); + close(socket_fd); + return; + } + + mavlink_message_t message {}; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN] {}; + + for (size_t i = 0; i < size; i += sizeof(message)) { + + const size_t copy_len = std::min(sizeof(message), size - i); + //printf("copy_len: %zu, %zu (%zu)\n", i, copy_len, size); + memcpy(reinterpret_cast(&message), data + i, copy_len); + + const ssize_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); + + struct sockaddr_in dest_addr {}; + dest_addr.sin_family = AF_INET; + + inet_pton(AF_INET, "127.0.0.1", &dest_addr.sin_addr.s_addr); + dest_addr.sin_port = htons(14556); + + if (sendto(socket_fd, buffer, buffer_len, 0, reinterpret_cast(&dest_addr), + sizeof(dest_addr)) != buffer_len) { + PX4_ERR("sendto error: %s", strerror(errno)); + } + } + + + close(socket_fd); +} diff --git a/platforms/posix/src/px4/common/print_load.cpp b/platforms/posix/src/px4/common/print_load.cpp index d385df4fda1b..4cfce909d0a7 100644 --- a/platforms/posix/src/px4/common/print_load.cpp +++ b/platforms/posix/src/px4/common/print_load.cpp @@ -72,7 +72,7 @@ void init_print_load(struct print_load_s *s) s->new_time = hrt_absolute_time(); s->interval_start_time = s->new_time; - for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + for (size_t i = 0; i < sizeof(s->last_times) / sizeof(s->last_times[0]); i++) { s->last_times[i] = 0; } @@ -116,7 +116,6 @@ void print_load(int fd, struct print_load_s *print_state) mach_msg_type_number_t thread_info_count; thread_basic_info_t basic_info_th; - uint32_t stat_thread = 0; // get all threads of the PX4 main task kr = task_threads(task_handle, &thread_list, &th_cnt); @@ -126,10 +125,6 @@ void print_load(int fd, struct print_load_s *print_state) return; } - if (th_cnt > 0) { - stat_thread += th_cnt; - } - long tot_sec = 0; long tot_usec = 0; long tot_cpu = 0; diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp index 82d8021a3a80..c17bbda26749 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2015-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 @@ -43,28 +43,32 @@ #include #include #include +#include #include +#include +#include +#include #include "pxh.h" namespace px4_daemon { -Pxh *Pxh::_instance = nullptr; - apps_map_type Pxh::_apps = {}; - +Pxh *Pxh::_instance = nullptr; Pxh::Pxh() { _history.try_to_add("commander takeoff"); // for convenience _history.reset_to_end(); - _instance = this; } Pxh::~Pxh() { - _instance = nullptr; + if (_local_terminal) { + tcsetattr(0, TCSANOW, &_orig_term); + _instance = nullptr; + } } int Pxh::process_line(const std::string &line, bool silently_fail) @@ -133,11 +137,167 @@ int Pxh::process_line(const std::string &line, bool silently_fail) } } +void Pxh::_check_remote_uorb_command(std::string &line) +{ -void Pxh::run_pxh() + if (line.empty()) { + return; + } + + std::stringstream line_stream(line); + std::string word; + + line_stream >> word; + + if (word == "uorb") { + line += " -1"; // Run uorb command only once + } +} + +void Pxh::run_remote_pxh(int remote_in_fd, int remote_out_fd) { - _should_exit = false; + std::string mystr; + int p1[2], pipe_stdout; + int p2[2], pipe_stderr; + int backup_stdout_fd = dup(STDOUT_FILENO); + int backup_stderr_fd = dup(STDERR_FILENO); + + if (pipe(p1) != 0) { + perror("Remote shell pipe creation failed"); + return; + } + + if (pipe(p2) != 0) { + perror("Remote shell pipe 2 creation failed"); + close(p1[0]); + close(p1[1]); + return; + } + + // Create pipe to receive stdout and stderr + dup2(p1[1], STDOUT_FILENO); + close(p1[1]); + + dup2(p2[1], STDERR_FILENO); + close(p2[1]); + + pipe_stdout = p1[0]; + pipe_stderr = p2[0]; + + // Set fds for non-blocking operation + fcntl(pipe_stdout, F_SETFL, fcntl(pipe_stdout, F_GETFL) | O_NONBLOCK); + fcntl(pipe_stderr, F_SETFL, fcntl(pipe_stderr, F_GETFL) | O_NONBLOCK); + fcntl(remote_in_fd, F_SETFL, fcntl(remote_in_fd, F_GETFL) | O_NONBLOCK); + + // Check for input on any pipe (i.e. stdout, stderr, or remote_in_fd + // stdout and stderr will be sent to the local terminal and a copy of the data + // will be sent over to the mavlink shell through the remote_out_fd. + // + // Any data from remote_in_fd will be process as shell commands when an '\n' is received + while (!_should_exit) { + + struct pollfd fds[3] { {pipe_stderr, POLLIN}, {pipe_stdout, POLLIN}, {remote_in_fd, POLLIN}}; + + if (poll(fds, 3, -1) == -1) { + perror("Mavlink Shell Poll Error"); + break; + } + if (fds[0].revents & POLLIN) { + + uint8_t buffer[512]; + size_t len; + + if ((len = read(pipe_stderr, buffer, sizeof(buffer))) <= 0) { + break; //EOF or ERROR + } + + // Send all the stderr data to the local terminal as well as the remote shell + if (write(backup_stderr_fd, buffer, len) <= 0) { + perror("Remote shell write stdout"); + break; + } + + if (write(remote_out_fd, buffer, len) <= 0) { + perror("Remote shell write"); + break; + } + + // Process all the stderr data first + continue; + } + + if (fds[1].revents & POLLIN) { + + uint8_t buffer[512]; + size_t len; + + if ((len = read(pipe_stdout, buffer, sizeof(buffer))) <= 0) { + break; //EOF or ERROR + } + + // Send all the stdout data to the local terminal as well as the remote shell + if (write(backup_stdout_fd, buffer, len) <= 0) { + perror("Remote shell write stdout"); + break; + } + + if (write(remote_out_fd, buffer, len) <= 0) { + perror("Remote shell write"); + break; + } + } + + if (fds[2].revents & POLLIN) { + + char c; + + if (read(remote_in_fd, &c, 1) <= 0) { + break; // EOF or ERROR + } + + switch (c) { + + case '\n': // user hit enter + printf("\n"); + _check_remote_uorb_command(mystr); + process_line(mystr, false); + // reset string + mystr = ""; + + _print_prompt(); + + break; + + default: // any other input + if (c > 3) { + fprintf(stdout, "%c", c); + fflush(stdout); + mystr += (char)c; + } + + break; + } + } + } + + // Restore stdout and stderr + dup2(backup_stdout_fd, STDOUT_FILENO); + dup2(backup_stderr_fd, STDERR_FILENO); + close(backup_stdout_fd); + close(backup_stderr_fd); + + close(pipe_stdout); + close(pipe_stderr); + close(remote_in_fd); + close(remote_out_fd); +} + +void Pxh::run_pxh() +{ + // Only the local_terminal needed for static calls + _instance = this; + _local_terminal = true; _setup_term(); std::string mystr; @@ -154,7 +314,10 @@ void Pxh::run_pxh() switch (c) { case EOF: - _should_exit = true; + break; + + case '\t': + _tab_completion(mystr); break; case 127: // backslash @@ -230,7 +393,6 @@ void Pxh::run_pxh() break; } - if (update_prompt) { // reprint prompt with mystr mystr.insert(mystr.length() - cursor_position, add_string); @@ -243,14 +405,11 @@ void Pxh::run_pxh() _move_cursor(cursor_position); } } - } } void Pxh::stop() { - _restore_term(); - if (_instance) { _instance->_should_exit = true; } @@ -294,4 +453,101 @@ void Pxh::_move_cursor(int position) printf("\033[%dD", position); } +void Pxh::_tab_completion(std::string &mystr) +{ + // parse line and get command + std::stringstream line(mystr); + std::string cmd; + line >> cmd; + + // cmd is empty or white space send a list of available commands + if (cmd.size() == 0) { + + printf("\n"); + + for (auto it = _apps.begin(); it != _apps.end(); ++it) { + printf("%s ", it->first.c_str()); + } + + printf("\n"); + mystr = ""; + + } else { + + // find tab completion matches + std::vector matches; + + for (auto it = _apps.begin(); it != _apps.end(); ++it) { + if (it->first.compare(0, cmd.size(), cmd) == 0) { + matches.push_back(it->first); + } + } + + if (matches.size() >= 1) { + // if more than one match print all matches + if (matches.size() != 1) { + printf("\n"); + + for (const auto &item : matches) { + printf("%s ", item.c_str()); + } + + printf("\n"); + } + + // find minimum size match + size_t min_size = 0; + + for (const auto &item : matches) { + if (min_size == 0) { + min_size = item.size(); + + } else if (item.size() < min_size) { + min_size = item.size(); + } + } + + // parse through elements to find longest match + std::string longest_match; + bool done = false; + + for (int i = 0; i < (int)min_size ; ++i) { + bool first_time = true; + + for (const auto &item : matches) { + if (first_time) { + longest_match += item[i]; + first_time = false; + + } else if (longest_match[i] != item[i]) { + done = true; + longest_match.pop_back(); + break; + } + } + + if (done) { break; } + + mystr = longest_match; + } + } + + std::string flags; + + while (line >> cmd) { + flags += " " + cmd; + } + + // add flags back in when there is a command match + if (matches.size() == 1) { + if (flags.empty()) { + mystr += " "; + + } else { + mystr += flags; + } + } + } +} + } // namespace px4_daemon diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.h b/platforms/posix/src/px4/common/px4_daemon/pxh.h index 2139277530f3..45bd4b863343 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.h +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016-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 @@ -72,7 +72,12 @@ class Pxh void run_pxh(); /** - * Can be called to stop pxh. + * Run the remote mavlink pxh shell. + */ + void run_remote_pxh(int remote_in_fd, int remote_out_fd); + + /** + * Can be called to stop all pxh shells. */ static void stop(); @@ -80,11 +85,14 @@ class Pxh void _print_prompt(); void _move_cursor(int position); void _clear_line(); + void _tab_completion(std::string &prefix); + void _check_remote_uorb_command(std::string &line); void _setup_term(); static void _restore_term(); bool _should_exit{false}; + bool _local_terminal{false}; History _history; struct termios _orig_term {}; diff --git a/platforms/posix/src/px4/common/px4_daemon/server.cpp b/platforms/posix/src/px4/common/px4_daemon/server.cpp index 1f1762bdf3e3..dab591cb3a8d 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server.cpp @@ -150,8 +150,12 @@ Server::_server_main() int n_ready = poll(poll_fds.data(), poll_fds.size(), -1); if (n_ready < 0) { - PX4_ERR("poll() failed: %s", strerror(errno)); - return; + // Reboot command causes System Interrupt to stop poll(). This is not an error + if (errno != EINTR) { + PX4_ERR("poll() failed: %s", strerror(errno)); + } + + break; } _lock(); diff --git a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp index b0c73fcb5b09..d06aa1922f62 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp @@ -73,19 +73,6 @@ FILE *get_stdout(bool *isatty_) return stdout; } -#ifdef __PX4_POSIX_EAGLE - - // XXX FIXME: thread_data_ptr is set to 0x1 in the main thread on Snapdragon - // even though the pthread_key has been created. - // We can catch this using the check below but we have no clue why this happens. - if (thread_data_ptr == (void *)0x1) { - if (isatty_) { *isatty_ = isatty(1); } - - return stdout; - } - -#endif - if (thread_data_ptr->thread_stdout == nullptr) { if (isatty_) { *isatty_ = isatty(1); } diff --git a/platforms/posix/src/px4/common/shmem_posix.cpp b/platforms/posix/src/px4/common/shmem_posix.cpp deleted file mode 100644 index 8f97d3f2b454..000000000000 --- a/platforms/posix/src/px4/common/shmem_posix.cpp +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Vijay Venkatraman. 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 -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include "px4muorb.h" - -//#define SHMEM_DEBUG - -static uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -extern unsigned char *adsp_changed_index; - -struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; -}; - -/*update value and param's change bit in shared memory*/ -void update_to_shmem(param_t param, union param_value_u value) -{ - if (px4muorb_param_update_to_shmem(param, (unsigned char *) &value, sizeof(value))) { - PX4_ERR("krait update param %u failed", param); - } -} - -void update_index_from_shmem(void) -{ - if (!adsp_changed_index) { - PX4_ERR("%s no param buffer", __FUNCTION__); - return; - } - - px4muorb_param_update_index_from_shmem(adsp_changed_index, PARAM_BUFFER_SIZE); -} - -static void update_value_from_shmem(param_t param, union param_value_u *value) -{ - if (px4muorb_param_update_value_from_shmem(param, (unsigned char *) value, sizeof(union param_value_u))) { - PX4_ERR("%s get param failed", __FUNCTION__); - } -} - -int update_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - unsigned int retval = 0; - - if (!adsp_changed_index) { - PX4_ERR("%s no param buffer", __FUNCTION__); - return 0; - } - - update_from_shmem_current_time = hrt_absolute_time(); - - if ((update_from_shmem_current_time - update_from_shmem_prev_time) - > 1000000) { //update every 1 second - update_from_shmem_prev_time = update_from_shmem_current_time; - update_index_from_shmem(); - } - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - - if (adsp_changed_index[byte_changed] & bit_changed) { - update_value_from_shmem(param, value); - adsp_changed_index[byte_changed] &= ~bit_changed; //clear the bit - retval = 1; - } - - //else {PX4_INFO("no change to param %s", param_name(param));} - - PX4_DEBUG("%s %d bit on adsp index[%d]", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); - - return retval; -} diff --git a/platforms/posix/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h b/platforms/posix/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h index 82e42c188238..f0523f39ddf1 100644 --- a/platforms/posix/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h +++ b/platforms/posix/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h @@ -35,6 +35,7 @@ #include +#if defined(CONFIG_I2C) static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) { @@ -51,3 +52,4 @@ static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) ret.is_external = true; return ret; } +#endif // CONFIG_I2C diff --git a/platforms/posix/src/px4/generic/generic/include/px4_arch/spi_hw_description.h b/platforms/posix/src/px4/generic/generic/include/px4_arch/spi_hw_description.h index 2ecdec5ebb7e..3ab4d9f3ee5f 100644 --- a/platforms/posix/src/px4/generic/generic/include/px4_arch/spi_hw_description.h +++ b/platforms/posix/src/px4/generic/generic/include/px4_arch/spi_hw_description.h @@ -35,6 +35,8 @@ #include +#if defined(CONFIG_SPI) + static inline constexpr px4_spi_bus_device_t initSPIDevice(uint8_t devid_driver, uint8_t cs_index) { px4_spi_bus_device_t ret{}; @@ -57,3 +59,4 @@ static inline constexpr px4_spi_bus_t initSPIBus(int bus, const px4_spi_bus_devi ret.requires_locking = false; return ret; } +#endif // CONFIG_SPI diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt deleted file mode 100644 index a7bf2971264e..000000000000 --- a/platforms/qurt/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ - -px4_add_git_submodule(TARGET git_dspal PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal") - -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal/cmake_hexagon") -include(toolchain/Toolchain-qurt) -include(fastrpc) -include(qurt_lib) -include(qurt_flags) - -include_directories(${CMAKE_CURRENT_BINARY_DIR}) - -get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) - -px4_qurt_generate_builtin_commands( - OUT ${PX4_BINARY_DIR}/apps - MODULE_LIST ${module_libraries} -) - -FASTRPC_STUB_GEN(px4muorb.idl) - -add_definitions(-D__QAIC_SKEL_EXPORT=__EXPORT) - -# Enable build without HexagonSDK to check link dependencies -if ("${QURT_ENABLE_STUBS}" STREQUAL "1") - - add_definitions(-D QURT_EXE_BUILD=1) - - include_directories( - ${CMAKE_CURRENT_BINARY_DIR} - ${FASTRPC_DSP_INCLUDES} - ) - - add_executable(px4 - ${PX4_BINARY_DIR}/apps.cpp - ${PX4_BINARY_DIR}/platforms/qurt/px4muorb_skel.c - ) - - target_link_libraries(px4 PRIVATE ${module_libraries}) - -else() - # Generate the DSP lib and stubs but not the apps side executable - # The Apps side executable is generated via the posix_eagle_xxxx target - QURT_LIB(LIB_NAME px4 - IDL_NAME px4muorb - SOURCES - ${PX4_BINARY_DIR}/apps.cpp - LINK_LIBS - modules__muorb__adsp - ${module_libraries} - m - ) - -endif() - -# board defined upload helper -if(EXISTS "${PX4_BOARD_DIR}/cmake/upload.cmake") - include(${PX4_BOARD_DIR}/cmake/upload.cmake) -endif() diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake deleted file mode 100644 index 054ab22c65b2..000000000000 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ /dev/null @@ -1,188 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ - -#============================================================================= -# -# Defined functions in this file -# -# OS Specific Functions -# -# * px4_qurt_add_firmware -# * px4_qurt_generate_builtin_commands -# * px4_qurt_add_export -# * px4_qurt_generate_romfs -# -# Required OS Inteface Functions -# -# * px4_os_add_flags -# * px4_os_determine_build_chip -# * px4_os_prebuild_targets -# - -#============================================================================= -# -# px4_qurt_generate_builtin_commands -# -# This function generates the builtin_commands.c src for qurt -# -# Usage: -# px4_qurt_generate_builtin_commands( -# MODULE_LIST -# OUT ) -# -# Input: -# MODULE_LIST : list of modules -# -# Output: -# OUT : stem of generated apps.cpp/apps.h ("apps"). -# -# Example: -# px4_qurt_generate_builtin_commands( -# OUT MODULE_LIST px4_simple_app) -# -function(px4_qurt_generate_builtin_commands) - px4_parse_function_args( - NAME px4_qurt_generate_builtin_commands - ONE_VALUE OUT - MULTI_VALUE MODULE_LIST - REQUIRED MODULE_LIST OUT - ARGN ${ARGN}) - - set(builtin_apps_string) - set(builtin_apps_decl_string) - set(command_count 0) - foreach(module ${MODULE_LIST}) - foreach(property MAIN STACK_MAIN PRIORITY) - get_target_property(${property} ${module} ${property}) - endforeach() - if (MAIN) - set(builtin_apps_string - "${builtin_apps_string}\tapps[\"${MAIN}\"] = ${MAIN}_main;\n") - set(builtin_apps_decl_string - "${builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") - math(EXPR command_count "${command_count}+1") - endif() - endforeach() - configure_file(${PX4_SOURCE_DIR}/platforms/common/apps.cpp.in ${OUT}.cpp) - configure_file(${PX4_SOURCE_DIR}/platforms/common/apps.h.in ${OUT}.h) -endfunction() - -#============================================================================= -# -# px4_os_add_flags -# -# Set the qurt build flags. -# -# Usage: -# px4_os_add_flags() -# -function(px4_os_add_flags) - - set(DSPAL_ROOT platforms/qurt/dspal) - include_directories( - ${DSPAL_ROOT}/include - ${DSPAL_ROOT}/sys - ${DSPAL_ROOT}/sys/sys - - platforms/posix/include - platforms/qurt/include - ) - - add_definitions( - -D__PX4_POSIX - -D__PX4_QURT - ) - - add_compile_options( - -fPIC - -fmath-errno - - -Wno-unknown-warning-option - -Wno-cast-align - ) - - # Clear -rdynamic flag which fails for hexagon - set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS) - set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS) - -endfunction() - -#============================================================================= -# -# px4_os_determine_build_chip -# -# Sets PX4_CHIP and PX4_CHIP_MANUFACTURER. -# -# Usage: -# px4_os_determine_build_chip() -# -function(px4_os_determine_build_chip) - - # always use generic chip and chip manufacturer - set(PX4_CHIP "generic" CACHE STRING "PX4 Chip" FORCE) - set(PX4_CHIP_MANUFACTURER "generic" CACHE STRING "PX4 Chip Manufacturer" FORCE) - -endfunction() - -#============================================================================= -# -# px4_os_prebuild_targets -# -# This function generates os dependent targets -# -# Usage: -# px4_os_prebuild_targets( -# OUT -# BOARD -# ) -# -# Input: -# BOARD : board -# -# Output: -# OUT : the target list -# -# Example: -# px4_os_prebuild_targets(OUT target_list BOARD px4_fmu-v2) -# -function(px4_os_prebuild_targets) - px4_parse_function_args( - NAME px4_os_prebuild_targets - ONE_VALUE OUT BOARD - REQUIRED OUT - ARGN ${ARGN}) - - add_library(prebuild_targets INTERFACE) - add_dependencies(prebuild_targets DEPENDS uorb_headers) - -endfunction() diff --git a/platforms/qurt/dspal b/platforms/qurt/dspal deleted file mode 160000 index 0322a4e345e4..000000000000 --- a/platforms/qurt/dspal +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0322a4e345e48ea28cb1cee14a33033cdaf0b16a diff --git a/platforms/qurt/include/crc32.h b/platforms/qurt/include/crc32.h deleted file mode 100644 index 34e080b1c2e3..000000000000 --- a/platforms/qurt/include/crc32.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * include/crc.h - * - * Copyright (C) 2010 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. - * - ****************************************************************************/ - -#ifndef __INCLUDE_CRC32_H -#define __INCLUDE_CRC32_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -#ifdef __cplusplus -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -/**************************************************************************** - * Name: crc32part - * - * Description: - * Continue CRC calculation on a part of the buffer. - * - ****************************************************************************/ - -EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); - -/**************************************************************************** - * Name: crc32 - * - * Description: - * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' - * - ****************************************************************************/ - -EXTERN uint32_t crc32(const uint8_t *src, size_t len); - -#undef EXTERN -#ifdef __cplusplus -} -#endif - -#endif /* __INCLUDE_CRC32_H */ diff --git a/platforms/qurt/include/hrt_work.h b/platforms/qurt/include/hrt_work.h deleted file mode 100644 index 36b2446c1429..000000000000 --- a/platforms/qurt/include/hrt_work.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 - -#pragma once - -__BEGIN_DECLS - -extern sem_t _hrt_work_lock; -extern struct wqueue_s g_hrt_work; - -void hrt_work_queue_init(void); -int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay); -void hrt_work_cancel(struct work_s *work); - -static inline void hrt_work_lock(void); -static inline void hrt_work_unlock(void); - -static inline void hrt_work_lock() -{ - //PX4_INFO("hrt_work_lock"); - sem_wait(&_hrt_work_lock); -} - -static inline void hrt_work_unlock() -{ - //PX4_INFO("hrt_work_unlock"); - sem_post(&_hrt_work_lock); -} - -__END_DECLS - diff --git a/platforms/qurt/include/queue.h b/platforms/qurt/include/queue.h deleted file mode 100644 index d6315ca1723c..000000000000 --- a/platforms/qurt/include/queue.h +++ /dev/null @@ -1,134 +0,0 @@ -/************************************************************************ - * include/queue.h - * - * Copyright (C) 2007-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. - * - ************************************************************************/ - -#ifndef __INCLUDE_QUEUE_H -#define __INCLUDE_QUEUE_H - -/************************************************************************ - * Included Files - ************************************************************************/ - -#include - -#ifdef __cplusplus -#include // NULL -#else -#include // NULL -#endif - -/************************************************************************ - * Pre-processor Definitions - ************************************************************************/ - -#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) -#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) - -#define sq_next(p) ((p)->flink) -#define dq_next(p) ((p)->flink) -#define dq_prev(p) ((p)->blink) - -#define sq_empty(q) ((q)->head == NULL) -#define dq_empty(q) ((q)->head == NULL) - -#define sq_peek(q) ((q)->head) -#define dq_peek(q) ((q)->head) - -// Required for Linux -#define FAR - -/************************************************************************ - * Global Type Declarations - ************************************************************************/ - -struct sq_entry_s { - FAR struct sq_entry_s *flink; -}; -typedef struct sq_entry_s sq_entry_t; - -struct dq_entry_s { - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; -}; -typedef struct dq_entry_s dq_entry_t; - -struct sq_queue_s { - FAR sq_entry_t *head; - FAR sq_entry_t *tail; -}; -typedef struct sq_queue_s sq_queue_t; - -struct dq_queue_s { - FAR dq_entry_t *head; - FAR dq_entry_t *tail; -}; -typedef struct dq_queue_s dq_queue_t; - -/************************************************************************ - * Global Function Prototypes - ************************************************************************/ - -#ifdef __cplusplus -#define EXTERN extern "C" -extern "C" { -#else -#define EXTERN extern -#endif - -EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); -EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); -EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); - -EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); -EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); -EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); -EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); -EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); -EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); - -#undef EXTERN -#ifdef __cplusplus -} -#endif - -#endif /* __INCLUDE_QUEUE_H_ */ - diff --git a/platforms/qurt/include/qurt_log.h b/platforms/qurt/include/qurt_log.h deleted file mode 100644 index ad32e8999a51..000000000000 --- a/platforms/qurt/include/qurt_log.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 - -__BEGIN_DECLS - -__EXPORT extern uint64_t hrt_absolute_time(void); - -//void qurt_log(int level, const char *file, int line, const char *format, ...); - -// declaration to make the compiler happy. This symbol is part of the adsp static image. -void HAP_debug(const char *msg, int level, const char *filename, int line); - -#ifndef qurt_log_defined -#define qurt_log_defined -static __inline void qurt_log(int level, const char *file, int line, - const char *format, ...) -{ - char buf[256]; - va_list args; - va_start(args, format); - vsnprintf(buf, sizeof(buf), format, args); - va_end(args); - HAP_debug(buf, level, file, line); -} -#endif - -__END_DECLS diff --git a/platforms/qurt/px4muorb.idl b/platforms/qurt/px4muorb.idl deleted file mode 100644 index c8872c09e312..000000000000 --- a/platforms/qurt/px4muorb.idl +++ /dev/null @@ -1,184 +0,0 @@ -//======================================================================= -// Copyright (c) 2016, Mark Charlebois. All rights reserved. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are -// met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * 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. -// * Neither the name of The Linux Foundation 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 "AS IS" AND ANY EXPRESS OR IMPLIED -// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT -// 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 "AEEStdDef.idl" - -interface px4muorb{ - - /** - * interface method to start the uorb service and initialize the muorb - */ - AEEResult orb_initialize(); - - /** - * Interface to set an offset to hrt_absolute_time on the DSP. - * - * @param time_diff_us: time difference of the DSP clock to Linux clock. - * A positive number means the Linux clock is ahead of the DSP one. - */ - AEEResult set_absolute_time_offset( in long time_diff_us ); - - /** - * Interface to request hrt_absolute_time on the DSP. - * @param time_us: pointer to time in us - */ - AEEResult get_absolute_time(rout unsigned long long time_us); - - /** - * Interface to update param for krait. - * - * @param param: param index. - * @param value: param value. - */ - AEEResult param_update_to_shmem( in unsigned long param, in sequence value); - - /** - * Interface to update index for krait. - * @param data: param index array. - */ - AEEResult param_update_index_from_shmem( rout sequence data); - - /** - * Interface to get param value for krait. - * - * @param param: param index. - * @param value: param value. - */ - AEEResult param_update_value_from_shmem( in unsigned long param, rout sequence value); - - /** - * Interface called from krait to inform of a published topic. - * @param topic_name: name of the topic being advertised - */ - AEEResult topic_advertised(in string topic_name); - - /** - * Interface called from krait to inform of a published topic. - * @param topic_name: name of the topic being advertised - */ - AEEResult topic_unadvertised(in string topic_name); - - /** - * Interface to add a subscriber to the identified topic - * @param topic_name - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult add_subscriber( in string topic_name ); - - /** - * Interface to remove a subscriber for the identified topic. - * @param topic_name - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult remove_subscriber( in string topic_name ); - - /** - * Interface called from krait for topic data. - * @param topic_name - * @param data - * a sequence of bytes for the passed data stream. - * as per the HExagon Documention, the max size of this stream is 255 bytes. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult send_topic_data( in string topic_name, in sequence data ); - - /** - * Inteface to check if there are subscribers on the remote adsp client - * This inteface is required as the krait app can be restarted without adsp - * being re-started. In this scenario the krait app does not know if there - * is subscriber on the remote end(ie adsp). - * @param topic_name - * The name of the topic for which the subscription needs to be checked. - * @param rout int status - * The status of the subscription, 0=no-subscribers, 1 = more than one subscriber. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult is_subscriber_present( in string topic_name, rout long status ); - - /** - * Interface to receive data from adsp. Since there is only one interface, the different message - * types are identified by the msg_type field. - * @param msg_type - * The possible values are: - * 1 ==> add_subscriber - * 2 ==> remove_subscriber - * 3 ==> recieve_topic_data - * 4 ==> advertised_topic - * @note: for message types, 1 & 2, the data pointer returned is null with length of 0. - * @param topic_name - * The topic being returned. - * @param data - * the data stream - * @param bytes_returned - * The number of bytes returned in the byte buffer. - * @return status - * 0 == success - * all others is a failure. - */ - AEEResult receive_msg( rout long msg_type, rout string topic_name, rout sequence data, rout long bytes_returned ); - - /** - * Since the receive_msg is a blocking call, the client will not be able to peform a clean shutdown. Calling this - * function will unblock the receive msg an return an error code. - * Ideally this should be implemented as a timeout for the #receive_msg call. - * @param none - * @return status - * 0 = success - * all others is a failure. - **/ - AEEResult unblock_recieve_msg(); - - /** - * This interface will perform a bulk read from the adsp and return the data buffer. - * The structure of the messages is as follows - * +----------------+-----------+---------------+----------+----------------+ - * | Topic Name Len | datal_len |TopicName(nullterminated) | data bytes | - * +----------------+-----------+---------------+----------+----------------+ - * |<-- 2 bytes -->|<-2bytes-> |<-- topicnamelen bytes -->|<-datalenbytes->| - * +----------------+---------------------------+----------+----------------+ - * @param data - * The output buffer where the data needs to be copied - * @param bytes_returned - * The number of bytes the buffer is filled up by. - * @param topic_count - * The numbe of topics filled in the buffer. - * @return status - * 0 = success - * all others is a failure. - **/ - AEEResult receive_bulk_data( rout sequence data, rout long bytes_returned, rout long topic_count ); -}; diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt deleted file mode 100644 index 929788b587e2..000000000000 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ /dev/null @@ -1,37 +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. -# -############################################################################ - -add_subdirectory(common) - -add_subdirectory(${PX4_CHIP_MANUFACTURER}) - diff --git a/platforms/qurt/src/px4/common/CMakeLists.txt b/platforms/qurt/src/px4/common/CMakeLists.txt deleted file mode 100644 index c64e4765d194..000000000000 --- a/platforms/qurt/src/px4/common/CMakeLists.txt +++ /dev/null @@ -1,62 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ - -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") -include(hexagon_sdk) - -include_directories(${HEXAGON_8074_INCLUDES}) - -set(QURT_LAYER_SRCS - px4_qurt_impl.cpp - tasks.cpp - lib_crc32.c - drv_hrt.cpp - qurt_stubs.c - main.cpp - shmem_qurt.cpp - px4_init.cpp - ) - -if ("${QURT_ENABLE_STUBS}" STREQUAL "1") - list(APPEND QURT_LAYER_SRCS - stubs/posix.c - stubs/qurt.c - ) -endif() - -add_library(px4_layer - ${QURT_LAYER_SRCS} - ${CONFIG_SRC} -) -target_link_libraries(px4_layer PRIVATE work_queue) -add_dependencies(px4_layer uorb_msgs) # dataman requires mission diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c deleted file mode 100644 index 6519fcce3283..000000000000 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 commands_muorb_test.c - * Commands to run for the "qurt_muorb_test" config - * - * @author Mark Charlebois - */ - -const char *get_commands(void); -const char *get_commands() -{ - - static const char *commands = - "param set CAL_GYRO0_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" -// "rgbled start\n" -// "tone_alarm start\n" - "rc_update start\n" - "commander start --hil\n" - "sensors start\n" - "ekf2 start\n" - "mc_hover_thrust_estimator start\n" - "flight_mode_manager start\n" - "mc_pos_control start\n" - "mc_att_control start\n" - "mc_rate_control start\n" - "sleep 1\n" - "pwm_out_sim start\n" - "param set RC1_MAX 2015\n" - "param set RC1_MIN 996\n" - "param set RC1_TRIM 1502\n" - "param set RC1_REV -1\n" - "param set RC2_MAX 2016 \n" - "param set RC2_MIN 995\n" - "param set RC2_TRIM 1500\n" - "param set RC3_MAX 2003\n" - "param set RC3_MIN 992\n" - "param set RC3_TRIM 992\n" - "param set RC4_MAX 2011\n" - "param set RC4_MIN 997\n" - "param set RC4_TRIM 1504\n" - "param set RC4_REV -1\n" - "param set RC6_MAX 2016\n" - "param set RC6_MIN 992\n" - "param set RC6_TRIM 1504\n" - "param set RC_CHAN_CNT 8\n" - "param set RC_MAP_MODE_SW 5\n" - "param set RC_MAP_POSCTL_SW 7\n" - "param set RC_MAP_RETURN_SW 8\n" - "param set MC_YAW_P 1.5\n" - "param set MC_PITCH_P 3.0\n" - "param set MC_ROLL_P 3.0\n" - "param set MC_YAWRATE_P 0.2\n" - "param set MC_PITCHRATE_P 0.03\n" - "param set MC_ROLLRATE_P 0.03\n" - "param set ATT_W_ACC 0.0002\n" - "param set ATT_W_MAG 0.002\n" - "param set ATT_W_GYRO_BIAS 0.05\n" - "sleep 1\n" - - - "param set MAV_TYPE 2\n" - "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" - "list_files\n" - "list_tasks\n" - "sleep 10\n" - "list_tasks\n" - "sleep 10\n" - - ; - - return commands; - -} diff --git a/platforms/qurt/src/px4/common/drv_hrt.cpp b/platforms/qurt/src/px4/common/drv_hrt.cpp deleted file mode 100644 index 0abb553d6cf5..000000000000 --- a/platforms/qurt/src/px4/common/drv_hrt.cpp +++ /dev/null @@ -1,477 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012 - 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 drv_hrt.cpp - * - * High-resolution timer with callouts and timekeeping. - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include "hrt_work.h" - -// Intervals in usec -static constexpr unsigned HRT_INTERVAL_MIN = 50; -static constexpr unsigned HRT_INTERVAL_MAX = 50000000; - -/* - * Queue of callout entries. - */ -static struct sq_queue_s callout_queue; - -/* latency baseline (last compare value applied) */ -static uint64_t latency_baseline; - -/* timer count at interrupt (for latency purposes) */ -static uint64_t latency_actual; - -/* latency histogram */ -const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; -const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; - -static px4_sem_t _hrt_lock; -static struct work_s _hrt_work; - -static int32_t dsp_offset = 0; - -static void hrt_latency_update(); - -static void hrt_call_reschedule(); -static void hrt_call_invoke(); - -hrt_abstime hrt_absolute_time_offset() -{ - return 0; -} - -static void hrt_lock() -{ - px4_sem_wait(&_hrt_lock); -} - -static void hrt_unlock() -{ - px4_sem_post(&_hrt_lock); -} - -#include "dspal_time.h" - -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) -{ - /* do nothing right now */ - return 0; -} - -int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) -{ - return clock_gettime(clk_id, tp); -} - -/* - * Get absolute time. - */ -hrt_abstime hrt_absolute_time() -{ - struct timespec ts; - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - return ts_to_abstime(&ts) + dsp_offset; -} - -int hrt_set_absolute_time_offset(int32_t time_diff_us) -{ - dsp_offset = time_diff_us; - return 0; -} - -/* - * Convert a timespec to absolute time. - */ -hrt_abstime ts_to_abstime(const struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/* - * Compute the delta between a timestamp taken in the past - * and now. - * - * This function is safe to use even if the timestamp is updated - * by an interrupt during execution. - */ -hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then) -{ - // This is not atomic as the value on the application layer of POSIX is limited. - hrt_abstime delta = hrt_absolute_time() - *then; - return delta; -} - -/* - * Store the absolute time in an interrupt-safe fashion. - * - * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. - */ -void hrt_store_absolute_time(volatile hrt_abstime *t) -{ - *t = hrt_absolute_time(); -} - -/* - * If this returns true, the entry has been invoked and removed from the callout list, - * or it has never been entered. - * - * Always returns false for repeating callouts. - */ -bool hrt_called(struct hrt_call *entry) -{ - return (entry->deadline == 0); -} - -/* - * Remove the entry from the callout list. - */ -void hrt_cancel(struct hrt_call *entry) -{ - hrt_lock(); - sq_rem(&entry->link, &callout_queue); - entry->deadline = 0; - - /* if this is a periodic call being removed by the callout, prevent it from - * being re-entered when the callout returns. - */ - entry->period = 0; - hrt_unlock(); - // endif -} - -static void hrt_latency_update() -{ - uint16_t latency = latency_actual - latency_baseline; - unsigned index; - - /* bounded buckets */ - for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { - if (latency <= latency_buckets[index]) { - latency_counters[index]++; - return; - } - } - - /* catch-all at the end */ - latency_counters[index]++; -} - -/* - * initialise a hrt_call structure - */ -void hrt_call_init(struct hrt_call *entry) -{ - memset(entry, 0, sizeof(*entry)); -} - -/* - * delay a hrt_call_every() periodic call by the given number of - * microseconds. This should be called from within the callout to - * cause the callout to be re-scheduled for a later time. The periodic - * callouts will then continue from that new base time at the - * previously specified period. - */ -void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) -{ - entry->deadline = hrt_absolute_time() + delay; -} - -/* - * Initialise the HRT. - */ -void hrt_init() -{ - sq_init(&callout_queue); - - int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); - - if (sem_ret) { - PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); - } - - memset(&_hrt_work, 0, sizeof(_hrt_work)); -} - -static void -hrt_call_enter(struct hrt_call *entry) -{ - struct hrt_call *call, *next; - - call = (struct hrt_call *)sq_peek(&callout_queue); - - if ((call == nullptr) || (entry->deadline < call->deadline)) { - sq_addfirst(&entry->link, &callout_queue); - //if (call != nullptr) PX4_INFO("call enter at head, reschedule (%lu %lu)", entry->deadline, call->deadline); - /* we changed the next deadline, reschedule the timer event */ - hrt_call_reschedule(); - - } else { - do { - next = (struct hrt_call *)sq_next(&call->link); - - if ((next == nullptr) || (entry->deadline < next->deadline)) { - //lldbg("call enter after head\n"); - sq_addafter(&call->link, &entry->link, &callout_queue); - break; - } - } while ((call = next) != nullptr); - } -} - -/** - * Timer interrupt handler - * - * This routine simulates a timer interrupt handler - */ -static void -hrt_tim_isr(void *p) -{ - /* grab the timer for latency tracking purposes */ - latency_actual = hrt_absolute_time(); - - /* do latency calculations */ - hrt_latency_update(); - - /* run any callouts that have met their deadline */ - hrt_call_invoke(); - - hrt_lock(); - - /* and schedule the next interrupt */ - hrt_call_reschedule(); - - hrt_unlock(); -} - -/** - * Reschedule the next timer interrupt. - * - * This routine must be called with interrupts disabled. - */ -static void -hrt_call_reschedule() -{ - hrt_abstime now = hrt_absolute_time(); - hrt_abstime delay = HRT_INTERVAL_MAX; - struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); - hrt_abstime deadline = now + HRT_INTERVAL_MAX; - - /* - * Determine what the next deadline will be. - * - * Note that we ensure that this will be within the counter - * period, so that when we truncate all but the low 16 bits - * the next time the compare matches it will be the deadline - * we want. - * - * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in - * hrt_absolute_time runs at least once per timer period. - */ - if (next != nullptr) { - //lldbg("entry in queue\n"); - if (next->deadline <= (now + HRT_INTERVAL_MIN)) { - //lldbg("pre-expired\n"); - /* set a minimal deadline so that we call ASAP */ - delay = HRT_INTERVAL_MIN; - - } else if (next->deadline < deadline) { - //lldbg("due soon\n"); - delay = next->deadline - now; - } - } - - /* set the new compare value and remember it for latency tracking */ - latency_baseline = now + delay; - - // There is no timer ISR, so simulate one by putting an event on the - // high priority work queue - - // Remove the existing expiry and update with the new expiry - hrt_work_cancel(&_hrt_work); - - hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay); -} - -static void -hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) -{ - PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval); - hrt_lock(); - - //PX4_INFO("hrt_call_internal after lock"); - /* if the entry is currently queued, remove it */ - /* note that we are using a potentially uninitialised - entry->link here, but it is safe as sq_rem() doesn't - dereference the passed node unless it is found in the - list. So we potentially waste a bit of time searching the - queue for the uninitialised entry->link but we don't do - anything actually unsafe. - */ - if (entry->deadline != 0) { - sq_rem(&entry->link, &callout_queue); - } - - entry->deadline = deadline; - entry->period = interval; - entry->callout = callout; - entry->arg = arg; - - hrt_call_enter(entry); - hrt_unlock(); -} - -/* - * Call callout(arg) after delay has elapsed. - * - * If callout is nullptr, this can be used to implement a timeout by testing the call - * with hrt_called(). - */ -void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) -{ - //printf("hrt_call_after\n"); - hrt_call_internal(entry, - hrt_absolute_time() + delay, - 0, - callout, - arg); -} - -/* - * Call callout(arg) after delay, and then after every interval. - * - * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may - * jitter but should not drift. - */ -void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) -{ - hrt_call_internal(entry, - hrt_absolute_time() + delay, - interval, - callout, - arg); -} - -/* - * Call callout(arg) at absolute time calltime. - */ -void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) -{ - hrt_call_internal(entry, calltime, 0, callout, arg); -} - -static void -hrt_call_invoke() -{ - struct hrt_call *call; - hrt_abstime deadline; - - hrt_lock(); - - while (true) { - /* get the current time */ - hrt_abstime now = hrt_absolute_time(); - - call = (struct hrt_call *)sq_peek(&callout_queue); - - if (call == nullptr) { - break; - } - - if (call->deadline > now) { - break; - } - - sq_rem(&call->link, &callout_queue); - //PX4_INFO("call pop"); - - /* save the intended deadline for periodic calls */ - deadline = call->deadline; - - /* zero the deadline, as the call has occurred */ - call->deadline = 0; - - /* invoke the callout (if there is one) */ - if (call->callout) { - // Unlock so we don't deadlock in callback - hrt_unlock(); - - //PX4_INFO("call %p: %p(%p)", call, call->callout, call->arg); - call->callout(call->arg); - - hrt_lock(); - } - - /* if the callout has a non-zero period, it has to be re-entered */ - if (call->period != 0) { - // re-check call->deadline to allow for - // callouts to re-schedule themselves - // using hrt_call_delay() - if (call->deadline <= now) { - call->deadline = deadline + call->period; - //PX4_INFO("call deadline set to %lu now=%lu", call->deadline, now); - } - - hrt_call_enter(call); - } - } - - hrt_unlock(); -} - -void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} diff --git a/platforms/qurt/src/px4/common/get_commands.h b/platforms/qurt/src/px4/common/get_commands.h deleted file mode 100644 index 2e89e67bbf37..000000000000 --- a/platforms/qurt/src/px4/common/get_commands.h +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * Copyright (C) 2015 Mark Charlebois. 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 get_commands.cpp - * functions to call to run the set of startup commands - * - * @author Mark Charlebois - */ - -#pragma once - -__BEGIN_DECLS -// The commands to run are specified in a target file: commands_.c -extern const char *get_commands(void); - -// Enable external library hook -void qurt_external_hook(void) __attribute__((weak)); -__END_DECLS - diff --git a/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h b/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h deleted file mode 100644 index d09b73c687dd..000000000000 --- a/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h +++ /dev/null @@ -1,36 +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. - * - ****************************************************************************/ -#pragma once - -#include - diff --git a/platforms/qurt/src/px4/common/lib_crc32.c b/platforms/qurt/src/px4/common/lib_crc32.c deleted file mode 100644 index c14ebfceeba2..000000000000 --- a/platforms/qurt/src/px4/common/lib_crc32.c +++ /dev/null @@ -1,125 +0,0 @@ -/************************************************************************************************ - * libc/misc/lib_crc32.c - * - * This file is a part of NuttX: - * - * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. - * - * The logic in this file was developed by Gary S. Brown: - * - * COPYRIGHT (C) 1986 Gary S. Brown. You may use this program, or code or tables - * extracted from it, as desired without restriction. - * - * First, the polynomial itself and its table of feedback terms. The polynomial is: - * - * X^32+X^26+X^23+X^22+X^16+X^12+X^11+X^10+X^8+X^7+X^5+X^4+X^2+X^1+X^0 - * - * Note that we take it "backwards" and put the highest-order term in the lowest-order bit. - * The X^32 term is "implied"; the LSB is the X^31 term, etc. The X^0 term (usually shown - * as "+1") results in the MSB being 1 - * - * Note that the usual hardware shift register implementation, which is what we're using - * (we're merely optimizing it by doing eight-bit chunks at a time) shifts bits into the - * lowest-order term. In our implementation, that means shifting towards the right. Why - * do we do it this way? Because the calculated CRC must be transmitted in order from - * highest-order term to lowest-order term. UARTs transmit characters in order from LSB - * to MSB. By storing the CRC this way we hand it to the UART in the order low-byte to - * high-byte; the UART sends each low-bit to hight-bit; and the result is transmission bit - * by bit from highest- to lowest-order term without requiring any bit shuffling on our - * part. Reception works similarly - * - * The feedback terms table consists of 256, 32-bit entries. Notes - * - * - The table can be generated at runtime if desired; code to do so is shown later. It - * might not be obvious, but the feedback terms simply represent the results of eight - * shift/xor operations for all combinations of data and CRC register values - * - * - The values must be right-shifted by eight bits by the updcrc logic; the shift must - * be u_(bring in zeroes). On some hardware you could probably optimize the shift in - * assembler by using byte-swap instructions polynomial $edb88320 - ************************************************************************************************/ - -/************************************************************************************************ - * Included Files - ************************************************************************************************/ - -#include -#include -#include - -// Needed for Linux -#define FAR - -/************************************************************************************************ - * Private Data - ************************************************************************************************/ - -static const uint32_t crc32_tab[] = { - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d -}; - -/************************************************************************************************ - * Public Functions - ************************************************************************************************/ -/************************************************************************************************ - * Name: crc32part - * - * Description: - * Continue CRC calculation on a part of the buffer. - * - ************************************************************************************************/ - -uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) -{ - size_t i; - - for (i = 0; i < len; i++) { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - - return crc32val; -} - -/************************************************************************************************ - * Name: crc32 - * - * Description: - * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' - * - ************************************************************************************************/ - -uint32_t crc32(FAR const uint8_t *src, size_t len) -{ - return crc32part(src, len, 0); -} diff --git a/platforms/qurt/src/px4/common/main.cpp b/platforms/qurt/src/px4/common/main.cpp deleted file mode 100644 index a238dc33f515..000000000000 --- a/platforms/qurt/src/px4/common/main.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/**************************************************************************** - * Copyright (C) 2015-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 main.cpp - * - * Basic shell to execute builtin "apps" on QURT. - * - * @author Mark Charlebois - * @author Julian Oes - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "get_commands.h" -#include "apps.h" - -#define MAX_ARGS 8 // max number of whitespace separated args after app name - - -static px4_task_t g_dspal_task = -1; - -__BEGIN_DECLS -// The commands to run are specified in a target file: commands_.c -extern const char *get_commands(void); - -// Enable external library hook -void qurt_external_hook(void) __attribute__((weak)); -__END_DECLS - -void qurt_external_hook(void) -{ -} - -static void run_cmd(apps_map_type &apps, const std::vector &appargs) -{ - // command is appargs[0] - std::string command = appargs[0]; - - //replaces app.find with iterator code to avoid null pointer exception - for (apps_map_type::iterator it = apps.begin(); it != apps.end(); ++it) - if (it->first == command) { - // one for command name, one for null terminator - const char *arg[MAX_ARGS + 2]; - - unsigned int i = 0; - - if (appargs.size() > MAX_ARGS + 1) { - PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); - return; - } - - while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { - arg[i] = (char *)appargs[i].c_str(); - PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); - ++i; - } - - arg[i] = (char *)0; - - //PX4_DEBUG_PRINTF(i); - if (apps[command] == NULL) { - PX4_ERR("Null function !!\n"); - - } else { - apps[command](i, (char **)arg); - break; - } - - } -} - -void eat_whitespace(const char *&b, int &i) -{ - // Eat whitespace - while (b[i] == ' ' || b[i] == '\t') { ++i; } - - b = &b[i]; - i = 0; -} - -static void process_commands(apps_map_type &apps, const char *cmds) -{ - std::vector appargs; - int i = 0; - const char *b = cmds; - char arg[256]; - - // Eat leading whitespace - eat_whitespace(b, i); - - for (;;) { - // End of command line - if (b[i] == '\n' || b[i] == '\0') { - strncpy(arg, b, i); - arg[i] = '\0'; - appargs.push_back(arg); - - // If we have a command to run - if (appargs.size() > 0) { - PX4_DEBUG("Processing command: %s", appargs[0].c_str()); - - for (int ai = 1; ai < (int)appargs.size(); ai++) { - PX4_DEBUG(" > arg: %s", appargs[ai].c_str()); - } - - run_cmd(apps, appargs); - } - - appargs.clear(); - - if (b[i] == '\n') { - eat_whitespace(b, ++i); - continue; - - } else { - break; - } - } - - // End of arg - else if (b[i] == ' ') { - strncpy(arg, b, i); - arg[i] = '\0'; - appargs.push_back(arg); - eat_whitespace(b, ++i); - continue; - } - - ++i; - } -} - -namespace px4 -{ -extern void init_once(void); -}; - -__BEGIN_DECLS -int dspal_main(int argc, char *argv[]); -__END_DECLS - - -const char *get_commands() -{ - // All that needs to be started automatically on the DSP side - // are uorb and qshell. After that, everything else can get - // started from the main startup script on the Linux side. - static const char *commands = "qshell start\n"; - - return commands; -} - - -int dspal_entry(int argc, char *argv[]) -{ - PX4_INFO("In dspal_entry"); - apps_map_type apps; - init_app_map(apps); - px4::init_once(); - px4::init(argc, (char **)argv, "px4"); - process_commands(apps, get_commands()); - sleep(1); // give time for all commands to execute before starting external function - - if (qurt_external_hook) { - qurt_external_hook(); - } - - for (;;) { - sleep(1); - } - - return 0; -} - -static void usage() -{ - PX4_INFO("Usage: dspal {start |stop}"); -} - - -extern "C" { - - int dspal_main(int argc, char *argv[]) - { - int ret = 0; - - if (argc == 2 && strcmp(argv[1], "start") == 0) { - g_dspal_task = px4_task_spawn_cmd("dspal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - dspal_entry, - argv); - - } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_dspal_task < 0) { - PX4_WARN("start up thread not running"); - - } else { - px4_task_delete(g_dspal_task); - g_dspal_task = -1; - } - - } else { - usage(); - ret = -1; - } - - return ret; - } -} diff --git a/platforms/qurt/src/px4/common/px4_init.cpp b/platforms/qurt/src/px4/common/px4_init.cpp deleted file mode 100644 index 73bada3ae173..000000000000 --- a/platforms/qurt/src/px4/common/px4_init.cpp +++ /dev/null @@ -1,54 +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 -#include -#include -#include - -int px4_platform_init() -{ - hrt_init(); - - param_init(); - - px4::WorkQueueManagerStart(); - - uorb_start(); - - //px4_log_initialize(); - - return PX4_OK; -} diff --git a/platforms/qurt/src/px4/common/px4_qurt_impl.cpp b/platforms/qurt/src/px4/common/px4_qurt_impl.cpp deleted file mode 100644 index 1f8825d59cc1..000000000000 --- a/platforms/qurt/src/px4/common/px4_qurt_impl.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Mark Charlebois. 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 px4_linux_impl.cpp - * - * PX4 Middleware Wrapper Linux Implementation - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "hrt_work.h" - -//extern pthread_t _shell_task_id; - - -__BEGIN_DECLS -extern uint64_t get_ticks_per_us(); - -//long PX4_TICKS_PER_SEC = 1000L; - -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i = 0; i < sec; i++) { - usleep(1000000); - } - - return 0; -} - -extern void hrt_init(void); - -#if 0 -void qurt_log(const char *fmt, ...) -{ - va_list args; - va_start(args, fmt); - printf(fmt, args); - printf("n"); - va_end(args); -} -#endif - -//extern int _posix_init(void); - -__END_DECLS - -extern struct wqueue_s gwork[NWORKERS]; - - -namespace px4 -{ - -void init_once(void); - -void init_once(void) -{ - // Required for QuRT - //_posix_init(); - -// _shell_task_id = pthread_self(); -// PX4_INFO("Shell id is %lu", _shell_task_id); - - work_queues_init(); - hrt_work_queue_init(); - - px4_platform_init(); -} - -void init(int argc, char *argv[], const char *app_name) -{ - PX4_DEBUG("App name: %s\n", app_name); -} - -} - -/** Retrieve from the data manager store */ -ssize_t -dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -) -{ - return 0; -} - -/** write to the data manager store */ -ssize_t -dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - dm_persitence_t persistence, /* The persistence level of this item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -) -{ - return 0; -} - -size_t strnlen(const char *s, size_t maxlen) -{ - size_t i = 0; - - while (s[i] != '\0' && i < maxlen) { - ++i; - } - - return i; -} - -int fprintf(FILE *stream, const char *format, ...) -{ - PX4_ERR("Error: Calling unresolved symbol stub:[%s(%s,...)]", __FUNCTION__, format); - return 0; -} - -int fputc(int c, FILE *stream) -{ - return c; -} - -int putchar(int character) -{ - return character; -} diff --git a/platforms/qurt/src/px4/common/qurt_stubs.c b/platforms/qurt/src/px4/common/qurt_stubs.c deleted file mode 100644 index 77f87377594b..000000000000 --- a/platforms/qurt/src/px4/common/qurt_stubs.c +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 - -void block_indefinite(void); -void _Read_uleb(void); -void _Parse_fde_instr(void); -void _Parse_csd(void); -void _Valbytes(void); -void _Get_eh_data(void); -void _Parse_lsda(void); -void __cxa_guard_release(void); -void _Read_enc_ptr(void); -void _Read_sleb(void); -void __cxa_guard_acquire(void); -void __cxa_pure_virtual(void); - -void block_indefinite(void) -{ - sem_t forever; - sem_init(&forever, 0, 0); - sem_wait(&forever); -} - -void _Read_uleb(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_fde_instr(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_csd(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Valbytes(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Get_eh_data(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Parse_lsda(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_guard_release(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Read_enc_ptr(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void _Read_sleb(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_guard_acquire(void) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -void __cxa_pure_virtual() -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); -} - -float _Stof(const char *p0, char **p1, long p2) -{ - PX4_WARN("Error: Calling unresolved symbol stub[%s]", __FUNCTION__); - block_indefinite(); - return 0; -} - -void *bsearch(const void *key, const void *ptr, size_t count, size_t size, int (*comp)(const void *, const void *)) -{ - const char *first = (const char *)ptr; - - while (count > 1) { - size_t m = count / 2; - const char *middle_element = first + m * size; - int cmp_res = comp(middle_element, key); - - if (cmp_res > 0) { - count = m; - - } else if (cmp_res == 0) { - return (void *)middle_element; - - } else { - first = middle_element + size; - count = count - m - 1; - } - } - - if (count && comp(first, key) == 0) { - return (void *)first; - } - - return NULL; -} diff --git a/platforms/qurt/src/px4/common/shmem_qurt.cpp b/platforms/qurt/src/px4/common/shmem_qurt.cpp deleted file mode 100644 index cb9ed77d36c2..000000000000 --- a/platforms/qurt/src/px4/common/shmem_qurt.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 Vijay Venkatraman. 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 -#include -#include -#include -#include -#include - -#include - -#include -#include - -//#define SHMEM_DEBUG -//#define PARAM_LOCK_DEBUG - -static atomic_word_t mem_lock; - -static unsigned char *map_base, *virt_addr; -static uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0; -static unsigned char krait_changed_index[MAX_SHMEM_PARAMS / 8 + 1]; - -struct shmem_info *shmem_info_p; - -// Small helper to get log2 for ints -static unsigned log2_for_int(unsigned v) -{ - unsigned r = 0; - - while (v >>= 1) { - ++r; - } - - return r; -} - -struct param_wbuf_s { - union param_value_u val; - param_t param; - bool unsaved; -}; -extern struct param_wbuf_s *param_find_changed(param_t param); - -int get_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - unsigned int i = 0; - -#ifdef PARAM_LOCK_DEBUG - PX4_INFO("lock value %d before get from %s, line: %d\n", mem_lock.value, strrchr(caller_file_name, '/'), - caller_line_number); -#endif - - while (!atomic_compare_and_set(&mem_lock, 1, 0)) { - i++; - usleep(1000); - - if (i > 100) { - break; - } - } - - if (i > 100) { - PX4_INFO("Could not get lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); - return -1; - - } else { - PX4_DEBUG("Lock acquired, file name: %s, line number: %d\n", - caller_file_name, caller_line_number); - } - - return 0; //got the lock - -} - -void release_shmem_lock(const char *caller_file_name, int caller_line_number) -{ - atomic_set(&mem_lock, 1); - -#ifdef PARAM_LOCK_DEBUG - PX4_INFO("release lock, file name: %s, line number: %d.\n", - strrchr(caller_file_name, '/'), caller_line_number); -#endif - - return; -} - -void init_shared_memory(void) -{ - int i; - - if (shmem_info_p) { - return; - } - - //virt_addr = map_memory(MAP_ADDRESS); - map_base = (unsigned char *)calloc(MAP_SIZE, 1); //16KB - - if (map_base == NULL) { - PX4_INFO("adsp memory malloc failed\n"); - return; - } - - virt_addr = map_base; - shmem_info_p = (struct shmem_info *) virt_addr; - - atomic_init(&mem_lock, 1); - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->krait_changed_index[i] = 0; - } - - PX4_INFO("adsp memory mapped\n"); -} - -void copy_params_to_shmem(const param_info_s *param_info_base) -{ - param_t param; - unsigned int i; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_INFO("Could not get shmem lock\n"); - return; - } - - //else PX4_INFO("Got lock\n"); - - for (param = 0; param < param_count(); param++) { - //{PX4_INFO("writing to offset %d\n", (unsigned char*)(shmem_info_p->adsp_params[param].name)-(unsigned char*)shmem_info_p);} - struct param_wbuf_s *s = param_find_changed(param); - - if (s == NULL) { - shmem_info_p->params_val[param] = param_info_base[param].val; - } - - else { - shmem_info_p->params_val[param] = s->val; - } - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param)); - - } else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("%d: written %f for param %s to shared mem", param, shmem_info_p->params_val[param].f, param_name(param)); - } - -#endif - } - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - shmem_info_p->adsp_changed_index[i] = 0; - krait_changed_index[i] = 0; - } - - release_shmem_lock(__FILE__, __LINE__); - //PX4_INFO("Released lock\n"); - -} - -/*update value and param's change bit in shared memory*/ -void update_to_shmem(param_t param, union param_value_u value) -{ - unsigned int byte_changed, bit_changed; - - if (!handle_in_range(param)) { - return; - } - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - shmem_info_p->params_val[param] = value; - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->adsp_changed_index[byte_changed] |= bit_changed; - - //PX4_INFO("set %d bit on adsp index[%d] to %d\n", bit_changed, byte_changed, shmem_info_p->adsp_changed_index[byte_changed]); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Set value %d for param %s to shmem, set adsp index %d:%d\n", value.i, param_name(param), byte_changed, - bit_changed); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Set value %f for param %s to shmem, set adsp index %d:%d\n", value.f, param_name(param), byte_changed, - bit_changed); - } - -#endif - - release_shmem_lock(__FILE__, __LINE__); - -} - -void update_index_from_shmem(void) -{ - unsigned int i; - param_t params[MAX_SHMEM_PARAMS / 8 + 1]; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - // Check if any param has been changed. - if (krait_changed_index[i] != shmem_info_p->krait_changed_index[i]) { - - // If a param has changed, we need to find out which one. - // From the byte and bit that is different, we can resolve the param number. - unsigned bit = log2_for_int( - krait_changed_index[i] - ^ shmem_info_p->krait_changed_index[i]); - param_t param_to_get = i * 8 + bit; - - // Update our krait_changed_index as well. - krait_changed_index[i] = shmem_info_p->krait_changed_index[i]; - params[i] = param_to_get; - - } else { - params[i] = 0xFFFF; - } - } - - release_shmem_lock(__FILE__, __LINE__); - - // FIXME: this is a hack but it gets the param so that it gets added - // to the local list param_values in param_shmem.c. - for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) { - if (params[i] != 0xFFFF) { - int32_t dummy; - param_get(params[i], &dummy); - } - } -} - -static void update_value_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - - if (get_shmem_lock(__FILE__, __LINE__) != 0) { - PX4_ERR("Could not get shmem lock\n"); - return; - } - - *value = shmem_info_p->params_val[param]; - - /*also clear the index since we are holding the lock*/ - byte_changed = param / 8; - bit_changed = 1 << param % 8; - shmem_info_p->krait_changed_index[byte_changed] &= ~bit_changed; - - release_shmem_lock(__FILE__, __LINE__); - -#ifdef SHMEM_DEBUG - - if (param_type(param) == PARAM_TYPE_INT32) { - PX4_INFO("Got value %d for param %s from shmem, cleared krait index %d:%d\n", value->i, param_name(param), byte_changed, - bit_changed); - } - - else if (param_type(param) == PARAM_TYPE_FLOAT) { - PX4_INFO("Got value %f for param %s from shmem, cleared krait index %d:%d\n", value->f, param_name(param), byte_changed, - bit_changed); - } - -#endif -} - -int update_from_shmem(param_t param, union param_value_u *value) -{ - unsigned int byte_changed, bit_changed; - unsigned int retval = 0; - - if (!handle_in_range(param) || value == NULL) { - return retval; - } - - update_from_shmem_current_time = hrt_absolute_time(); - - if ((update_from_shmem_current_time - update_from_shmem_prev_time) - > 1000000) { //update every 1 second - update_from_shmem_prev_time = update_from_shmem_current_time; - update_index_from_shmem(); - } - - byte_changed = param / 8; - bit_changed = 1 << param % 8; - - if (krait_changed_index[byte_changed] & bit_changed) { - update_value_from_shmem(param, value); - krait_changed_index[byte_changed] &= ~bit_changed; - retval = 1; - } - - //else {PX4_INFO("no change to param %s\n", param_name(param));} - - PX4_DEBUG("%s %d bit on krait changed index[%d]\n", - (retval) ? "cleared" : "unchanged", bit_changed, byte_changed); - - return retval; -} diff --git a/platforms/qurt/src/px4/common/stubs/posix.c b/platforms/qurt/src/px4/common/stubs/posix.c deleted file mode 100644 index 24b22f13f913..000000000000 --- a/platforms/qurt/src/px4/common/stubs/posix.c +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include -#include -#include -#include - -int sem_init(sem_t *sem, int pshared, unsigned int value) -{ - return 1; -} - -int sem_wait(sem_t *sem) -{ - return 1; -} - -int sem_destroy(sem_t *sem) -{ - return 1; -} - -int sem_post(sem_t *sem) -{ - return 1; -} - -int sem_getvalue(sem_t *sem, int *sval) -{ - return 1; -} - -int usleep(useconds_t usec) -{ - return 0; -} - -pthread_t pthread_self(void) -{ - pthread_t x = 0; - return x; -} - - -int pthread_kill(pthread_t thread, int sig) -{ - return 1; -} - -void pthread_exit(void *retval) -{ -} - -int pthread_join(pthread_t thread, void **retval) -{ - return 1; -} - -int pthread_cancel(pthread_t thread) -{ - return 1; -} -int pthread_attr_init(pthread_attr_t *attr) -{ - return 1; -} - -int pthread_attr_setstacksize(pthread_attr_t *attr, size_t stacksize) -{ - return 1; -} - -int pthread_attr_getstacksize(const pthread_attr_t *attr, size_t *stacksize) -{ - return 1; -} - -int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *param) -{ - return 1; -} - -int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) -{ - return 1; -} -int pthread_attr_getschedparam(const pthread_attr_t *attr, struct sched_param *param) -{ - return 1; -} - -int pthread_attr_destroy(pthread_attr_t *attr) -{ - return 1; -} - -int clock_gettime(clockid_t clk_id, struct timespec *tp) -{ - return 1; -} - -int pthread_mutex_lock(pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_mutex_unlock(pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_cond_signal(pthread_cond_t *cond) -{ - return 1; -} -int pthread_mutex_destroy(pthread_mutex_t *mutex) -{ - return 1; -} -int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr) -{ - return 1; -} - -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) -{ - return 1; -} - -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime) -{ - return 1; -} - -int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr) -{ - return 1; -} -int pthread_mutexattr_init(pthread_mutexattr_t *attr) -{ - return -1; -} -int pthread_mutexattr_destroy(pthread_mutexattr_t *attr) -{ - return -1; -} -int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type) -{ - return -1; -} - -int pthread_condattr_init(pthread_condattr_t *attr) -{ - return -1; -} - -int fsync(int fd) -{ - return -1; -} diff --git a/platforms/qurt/src/px4/common/stubs/qurt.c b/platforms/qurt/src/px4/common/stubs/qurt.c deleted file mode 100644 index 09a28f52b4df..000000000000 --- a/platforms/qurt/src/px4/common/stubs/qurt.c +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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 - -// This code is never run. It is used solely to test linking in the -// TravisCI build test to make sure all symbols are resolved. -__EXPORT int _start_main(void); - -void HAP_debug(const char *msg, int level, const char *filename, int line) -{ -} - -void HAP_power_request(int a, int b, int c) -{ -} - -int dlinit(int a, char **b) -{ - return 1; -} - -int main(int argc, char *argv[]) -{ - int ret = 0; - - return ret; -} - -int _start_main() -{ - return -1; -} diff --git a/platforms/qurt/src/px4/common/tasks.cpp b/platforms/qurt/src/px4/common/tasks.cpp deleted file mode 100644 index 2ec1d1c4dbb7..000000000000 --- a/platforms/qurt/src/px4/common/tasks.cpp +++ /dev/null @@ -1,402 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015-2016 Mark Charlebois. 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 px4_qurt_tasks.c - * Implementation of existing task API for QURT. - * - * @author Mark Charlebois - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if !defined(__PX4_QURT) -#include -#endif - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#define MAX_CMD_LEN 100 - -#define PX4_MAX_TASKS 50 -#define SHELL_TASK_ID (PX4_MAX_TASKS+1) - -pthread_t _shell_task_id = 0; - -struct task_entry { - pthread_t pid; - std::string name; - bool isused; - task_entry() : isused(false) {} -}; - -static task_entry taskmap[PX4_MAX_TASKS]; - -typedef struct { - px4_main_t entry; - int argc; - char *argv[]; - // strings are allocated after the -} pthdata_t; - -static void *entry_adapter(void *ptr) -{ - pthdata_t *data; - data = (pthdata_t *) ptr; - - data->entry(data->argc, data->argv); - //PX4_WARN("Before waiting infinte busy loop"); - //for( ;; ) - //{ - // volatile int x = 0; - // ++x; - // } - free(ptr); - px4_task_exit(0); - - return NULL; -} - -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, - char *const argv[]) -{ - struct sched_param param; - pthread_attr_t attr; - pthread_t task; - int rv; - int argc = 0; - int i; - unsigned int len = 0; - unsigned long offset; - unsigned long structsize; - char *p = (char *)argv; - - PX4_DEBUG("Creating %s\n", name); - PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); - - // Calculate argc - while (p != (char *)0) { - p = argv[argc]; - - if (p == (char *)0) { - break; - } - - ++argc; - len += strlen(p) + 1; - } - - structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); - pthdata_t *taskdata = nullptr; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize + len); - - if (taskdata == nullptr) { - return -ENOMEM; - } - - offset = ((unsigned long)taskdata) + structsize; - - taskdata->entry = entry; - taskdata->argc = argc; - - for (i = 0; i < argc; i++) { - PX4_DEBUG("arg %d %s\n", i, argv[i]); - taskdata->argv[i] = (char *)offset; - strcpy((char *)offset, argv[i]); - offset += strlen(argv[i]) + 1; - } - - // Must add NULL at end of argv - taskdata->argv[argc] = (char *)0; - - rv = pthread_attr_init(&attr); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); - return (rv < 0) ? rv : -rv; - } - - PX4_DEBUG("stack address after pthread_attr_init: 0x%X", attr.stackaddr); - PX4_DEBUG("attr address: 0x%X, param address: 0x%X", &attr, ¶m); - rv = pthread_attr_getschedparam(&attr, ¶m); - PX4_DEBUG("stack address after pthread_attr_getschedparam: 0x%X", attr.stackaddr); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to get thread sched param"); - return (rv < 0) ? rv : -rv; - } - -#if 0 - rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); - return (rv < 0) ? rv : -rv; - } - - rv = pthread_attr_setschedpolicy(&attr, scheduler); - - if (rv != 0) { - PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); - return (rv < 0) ? rv : -rv; - } - -#endif - size_t fixed_stacksize = -1; - pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_DEBUG("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); - fixed_stacksize = 8 * 1024; - fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; - - PX4_DEBUG("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); - pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(fixed_stacksize)); - - PX4_DEBUG("stack address after pthread_attr_setstacksize: 0x%X", attr.stackaddr); - param.sched_priority = priority; - - rv = pthread_attr_setschedparam(&attr, ¶m); - - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: failed to set sched param"); - return (rv < 0) ? rv : -rv; - } - - rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); - - if (rv != 0) { - PX4_ERR("px4_task_spawn_cmd: pthread_create failed, error: %d", rv); - return (rv < 0) ? rv : -rv; - } - - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused == false) { - taskmap[i].pid = task; - taskmap[i].name = name; - taskmap[i].isused = true; - break; - } - } - - if (i >= PX4_MAX_TASKS) { - return -ENOSPC; - } - - return i; -} - -int px4_task_delete(px4_task_t id) -{ - int rv = 0; - pthread_t pid; - PX4_WARN("Called px4_task_delete"); - - if (id < PX4_MAX_TASKS && taskmap[id].isused) { - pid = taskmap[id].pid; - - } else { - return -EINVAL; - } - - // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { - taskmap[id].isused = false; - pthread_exit(0); - - } else { - rv = pthread_cancel(pid); - } - - taskmap[id].isused = false; - - return rv; -} - -void px4_task_exit(int ret) -{ - int i; - pthread_t pid = pthread_self(); - - // Get pthread ID from the opaque ID - for (i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].pid == pid) { - taskmap[i].isused = false; - break; - } - } - - if (i >= PX4_MAX_TASKS) { - PX4_ERR("px4_task_exit: self task not found!"); - - } else { - PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); - } - - //pthread_exit((void *)(unsigned long)ret); -} - -int px4_task_kill(px4_task_t id, int sig) -{ - int rv = 0; - pthread_t pid; - PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { - pid = taskmap[id].pid; - - } else { - return -EINVAL; - } - - // If current thread then exit, otherwise cancel - rv = pthread_kill(pid, sig); - - return rv; -} - -void px4_show_tasks() -{ - int idx; - int count = 0; - - PX4_INFO("Active Tasks:"); - - for (idx = 0; idx < PX4_MAX_TASKS; idx++) { - if (taskmap[idx].isused) { - PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); - count++; - } - } - - if (count == 0) { - PX4_INFO(" No running tasks"); - } - -} - -px4_task_t px4_getpid() -{ - pthread_t pid = pthread_self(); -// -// if (pid == _shell_task_id) -// return SHELL_TASK_ID; - - // Get pthread ID from the opaque ID - for (int i = 0; i < PX4_MAX_TASKS; ++i) { - if (taskmap[i].isused && taskmap[i].pid == pid) { - return i; - } - } - - return ~0; -} - - -const char *px4_get_taskname() -{ - pthread_t pid = pthread_self(); - - for (int i = 0; i < PX4_MAX_TASKS; i++) { - if (taskmap[i].isused && taskmap[i].pid == pid) { - return taskmap[i].name.c_str(); - } - } - - return "Unknown App"; -} - -static void timer_cb(void *data) -{ - px4_sem_t *sem = reinterpret_cast(data); - - sem_post(sem); -} - -int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) -{ - work_s _hpwork = {}; - - // Get the current time. - struct timespec ts_now; - px4_clock_gettime(CLOCK_MONOTONIC, &ts_now); - - // We get an absolute time but want to calculate a timeout in us. - hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now); - - // Create a timer to unblock. - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us); - sem_wait(sem); - hrt_work_cancel(&_hpwork); - return 0; -} - -int px4_prctl(int option, const char *arg2, px4_task_t pid) -{ - int rv; - - switch (option) { - case PR_SET_NAME: - // set the threads name - Not supported - // rv = pthread_setname_np(pthread_self(), arg2); - rv = -1; - break; - - default: - rv = -1; - PX4_WARN("FAILED SETTING TASK NAME"); - break; - } - - return rv; -} diff --git a/platforms/qurt/src/px4/generic/CMakeLists.txt b/platforms/qurt/src/px4/generic/CMakeLists.txt deleted file mode 100644 index ec489bf559c3..000000000000 --- a/platforms/qurt/src/px4/generic/CMakeLists.txt +++ /dev/null @@ -1,36 +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. -# -############################################################################ - - -add_subdirectory(${PX4_CHIP}) - diff --git a/platforms/qurt/src/px4/generic/generic/CMakeLists.txt b/platforms/qurt/src/px4/generic/generic/CMakeLists.txt deleted file mode 100644 index c4e98943e42e..000000000000 --- a/platforms/qurt/src/px4/generic/generic/CMakeLists.txt +++ /dev/null @@ -1,35 +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. -# -############################################################################ - - - diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h deleted file mode 100644 index 82e42c188238..000000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h +++ /dev/null @@ -1,53 +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. - * - ****************************************************************************/ - -#pragma once - -#include - - -static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) -{ - px4_i2c_bus_t ret{}; - ret.bus = bus; - ret.is_external = false; - return ret; -} - -static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) -{ - px4_i2c_bus_t ret{}; - ret.bus = bus; - ret.is_external = true; - return ret; -} diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h deleted file mode 100644 index d09b73c687dd..000000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h +++ /dev/null @@ -1,36 +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. - * - ****************************************************************************/ -#pragma once - -#include - diff --git a/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h b/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h deleted file mode 100644 index 52836a1c7000..000000000000 --- a/platforms/qurt/src/px4/generic/generic/include/px4_arch/spi_hw_description.h +++ /dev/null @@ -1,59 +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. - * - ****************************************************************************/ - -#pragma once - -#include - -static inline constexpr px4_spi_bus_device_t initSPIDevice(uint8_t devid_driver) -{ - px4_spi_bus_device_t ret{}; - ret.cs_gpio = 1; // set to some non-zero value to indicate this is used - ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, 0); - ret.devtype_driver = devid_driver; - return ret; -} - -static inline constexpr px4_spi_bus_t initSPIBus(int bus, const px4_spi_bus_devices_t &devices) -{ - px4_spi_bus_t ret{}; - - for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { - ret.devices[i] = devices.devices[i]; - } - - ret.bus = bus; - ret.is_external = false; // all buses are marked internal on QuRT - ret.requires_locking = false; - return ret; -} diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index 702483c198d3..2214e4406fc1 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_imu_filtering b/posix-configs/SITL/init/test/test_imu_filtering new file mode 100644 index 000000000000..b64c8cc5d9de --- /dev/null +++ b/posix-configs/SITL/init/test/test_imu_filtering @@ -0,0 +1,58 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +param load +param set CBRK_SUPPLY_CHK 894281 + +dataman start + +tone_alarm start + +ver all + +param set IMU_GYRO_RATEMAX 1000 + +param set IMU_GYRO_FFT_EN 1 +param set IMU_GYRO_FFT_MIN 10 +param set IMU_GYRO_FFT_MAX 1000 +param set IMU_GYRO_FFT_LEN 512 +param set IMU_GYRO_FFT_SNR 10 + +# dynamic notches ESC/FFT/both +#param set IMU_GYRO_DNF_EN 1 +#param set IMU_GYRO_DNF_EN 2 +param set IMU_GYRO_DNF_EN 3 + +# test values +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 40 +#param set IMU_GYRO_NF0_FRQ 60 +#param set IMU_GYRO_NF0_BW 5 + +# log nearly everything +param set SDLOG_PROFILE 859 + +fake_imu start + +sensors start +gyro_fft start + +logger start -f -t -b 10000 -p sensor_gyro +logger on + +echo "Running for 10 seconds" +sleep 10 + +logger off + +sensors status +listener sensor_gyro +listener sensor_gyro_fifo +listener sensor_gyro_fft +perf + +echo "ALL TESTS PASSED" + +shutdown diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index d75544238a22..ad02885bd6c7 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 6f5649f9f25c..2eceb11f8d6e 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -5,11 +5,10 @@ param load -param set BAT_N_CELLS 3 +param set BAT1_N_CELLS 3 param set CBRK_SUPPLY_CHK 894281 param set MAV_TYPE 22 param set VT_TYPE 2 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index cef5c8b33f45..3db4251e3236 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -3,6 +3,8 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh +param select eeprom/parameters + dataman start simulator start diff --git a/posix-configs/SITL/init/test/tests_all b/posix-configs/SITL/init/test/tests_all index 063dee50dabe..1017bb6df7e1 100644 --- a/posix-configs/SITL/init/test/tests_all +++ b/posix-configs/SITL/init/test/tests_all @@ -5,7 +5,6 @@ param load param set CBRK_SUPPLY_CHK 894281 -param set SYS_RESTART_TYPE 0 dataman start diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index afe9f4bc5725..ef4de1e4fd89 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -6,7 +6,8 @@ # config for a quad # modified from ../rpi/px4.config -param load +param select eeprom/parameters +param import param set CBRK_SUPPLY_CHK 894281 @@ -41,7 +42,7 @@ param set MAV_TYPE 2 param set BAT1_V_CHANNEL 5 # Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11 -param set BAT_V_DIV 11.0 +param set BAT1_V_DIV 11.0 dataman start diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 15d506d48a2e..e5d7f58fd160 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -6,7 +6,8 @@ # config for fixed wing (FW) # modified from ./px4.config, switch att/pos_control & mixer -param load +param select eeprom/parameters +param import param set CBRK_SUPPLY_CHK 894281 @@ -41,7 +42,7 @@ param set MAV_TYPE 2 param set BAT1_V_CHANNEL 5 # Battery voltage scale factor, from BBBlue schematics: (4.7K + 47K) / 4.7K = 11 -param set BAT_V_DIV 11.0 +param set BAT1_V_DIV 11.0 dataman start diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config deleted file mode 100644 index 251eadb0bc2c..000000000000 --- a/posix-configs/eagle/200qx/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config deleted file mode 100644 index 90d35c64e82b..000000000000 --- a/posix-configs/eagle/200qx/px4.config +++ /dev/null @@ -1,38 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set SENS_BOARD_ROT 0 - - -sleep 1 -mpu9250 -s start -bmp280 -I start -gps start -d /dev/tty-4 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-2 -spektrum_rc start -d /dev/tty-1 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config deleted file mode 100644 index 251eadb0bc2c..000000000000 --- a/posix-configs/eagle/210qc/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config deleted file mode 100644 index 941a704607d2..000000000000 --- a/posix-configs/eagle/210qc/px4.config +++ /dev/null @@ -1,38 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set ATT_W_MAG 0.00 -param set SENS_BOARD_ROT 0 - -sleep 1 -mpu9250 -s start -bmp280 -I start -gps start -d /dev/tty-4 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-2 -spektrum_rc start -d /dev/tty-1 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config deleted file mode 100644 index 251eadb0bc2c..000000000000 --- a/posix-configs/eagle/flight/mainapp.config +++ /dev/null @@ -1,19 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -t -b 200 -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config deleted file mode 100644 index de030a021744..000000000000 --- a/posix-configs/eagle/flight/px4.config +++ /dev/null @@ -1,24 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -sleep 1 -gps start -d /dev/tty-4 -param set MAV_TYPE 2 -sleep 1 -hmc5883 start -mpu9250 start -bmp280 start -sleep 1 -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -# TODO -#linux_pwm_out start -spektrum_rc start diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config deleted file mode 100644 index b61bd58bbce3..000000000000 --- a/posix-configs/eagle/hil/mainapphil.config +++ /dev/null @@ -1,11 +0,0 @@ -muorb start -# Wait 1s before setting parameters for muorb to initialize -sleep 1 -param set CBRK_SUPPLY_CHK 894281 -mavlink start -x -u 14556 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink boot_complete -battery_simulator start -simulator start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config deleted file mode 100644 index f1afab09db04..000000000000 --- a/posix-configs/eagle/hil/px4.config +++ /dev/null @@ -1,32 +0,0 @@ -qshell start - -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set SYS_AUTOCONFIG 1 -param set MAV_TYPE 2 -param set MC_YAW_P 1.5 -param set MC_PITCH_P 3.0 -param set MC_ROLL_P 3.0 -param set MC_YAWRATE_P 0.2 -param set MC_PITCHRATE_P 0.03 -param set MC_ROLLRATE_P 0.03 - -sleep 1 -rc_update start -commander start -hil -sensors start -ekf2 start -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -land_detector start multicopter -sleep 1 -pwm_out_sim start -mixer load /dev/pwm_output0 /startup/quad_x.main.mix -list_files -list_tasks -sleep 10 -list_tasks -sleep 10 diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS deleted file mode 100644 index f8a0dca0c80f..000000000000 --- a/posix-configs/eagle/init/rcS +++ /dev/null @@ -1,103 +0,0 @@ -#!/bin/sh - -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start - -# We need to wait until the DSP side is ready before -# sending commands with qshell. -sleep 1 - -# default, generic quad platform -if param compare SYS_AUTOSTART 4100 -then - param set MAV_TYPE 2 - - qshell pwm_out_rc_in start -d /dev/tty-2 - - qshell gps start -d /dev/tty-4 - qshell hmc5883 start - -# Qualcomm 200qx microheli platform -elif param compare SYS_AUTOSTART 4200 -then - param set MAV_TYPE 2 - param set MC_YAW_P 7.0 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.0001 - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.0001 - param set SENS_BOARD_ROT 0 - -# Qualcomm internal 210qc platform -elif param compare SYS_AUTOSTART 4210 -then - param set MAV_TYPE 2 - param set MC_YAW_P 12 - param set MC_YAWRATE_P 0.08 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_D 0 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.001 - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.08 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.001 - param set SENS_BOARD_ROT 0 - -else - echo "NO AIRFRAME CHOSEN!" - echo "Please set parameter to select airframe:" - echo " SYS_AUTOSTART 4100: generic (flight) Quad" - echo " SYS_AUTOSTART 4200: Microheli 200QX" -fi - - -qshell mpu9250 start -qshell bmp280 start -qshell rc_update start -qshell sensors start - -if param compare SYS_MC_EST_GROUP 1 -then - qshell attitude_estimator_q start - qshell local_position_estimator start - -elif param compare SYS_MC_EST_GROUP 2 -then - param set EKF2_GBIAS_INIT 0.01 - param set EKF2_ANGERR_INIT 0.01 - qshell ekf2 start -else - echo "No estimator chosen" - exit -1 -fi - -qshell commander start -qshell land_detector start multicopter -qshell mc_hover_thrust_estimator start -qshell flight_mode_manager start -qshell mc_pos_control start -qshell mc_att_control start -qshell mc_rate_control start - -logger start -b 200 -t - -dataman start -navigator start -mavlink start -u 14556 -r 1000000 -p -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/excelsior/mainapp.config b/posix-configs/excelsior/mainapp.config deleted file mode 100644 index 4146bfd4e4c3..000000000000 --- a/posix-configs/excelsior/mainapp.config +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -muorb start -logger start -e -t -param set CBRK_SUPPLY_CHK 894281 -dataman start -navigator start -sleep 2 -mavlink start -x -u 14556 -r 1000000 -p -sleep 1 -mavlink stream -u 14556 -s HIGHRES_IMU -r 50 -mavlink stream -u 14556 -s ATTITUDE -r 50 -mavlink stream -u 14556 -s RC_CHANNELS -r 20 -mavlink boot_complete diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config deleted file mode 100644 index f634095b356b..000000000000 --- a/posix-configs/excelsior/px4.config +++ /dev/null @@ -1,36 +0,0 @@ -qshell start -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MC_YAW_P 12 -param set MC_YAWRATE_P 0.08 -param set MC_YAWRATE_I 0.0 -param set MC_YAWRATE_D 0 -param set MC_PITCH_P 7.0 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.0 -param set MC_PITCHRATE_D 0.001 -param set MC_ROLL_P 7.0 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.0 -param set MC_ROLLRATE_D 0.001 -param set SENS_BOARD_ROT 4 - -sleep 1 -mpu9250 -s start -bmp280 -I start -rc_update start -sensors start -commander start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start -uart_esc start -D /dev/tty-1 -spektrum_rc start -d /dev/tty-101 -sleep 1 -list_files -list_tasks diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config deleted file mode 100644 index 31d8482b1a81..000000000000 --- a/posix-configs/ocpoc/px4.config +++ /dev/null @@ -1,50 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -param select /root/rootfs/eeprom/parameters -if [ -f /root/rootfs/eeprom/parameters ] -then - param load -fi -param set CBRK_SUPPLY_CHK 894281 -param set SYS_AUTOSTART 4001 -param set MAV_TYPE 2 -param set MAV_SYS_ID 1 - -load_mon start - -mpu9250 -s -R 4 start -hmc5883 -I start -ms5611 -s start - -rgbled start -b 1 - -board_adc start -battery_status start - -gps start -d /dev/ttyS3 -s -rc_update start -sensors start -commander start -navigator start -dataman start -ekf2 start -land_detector start multicopter -mc_hover_thrust_estimator start -flight_mode_manager start -mc_pos_control start -mc_att_control start -mc_rate_control start - -mavlink start -x -d /dev/ttyPS1 -mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50 -mavlink stream -d /dev/ttyPS1 -s ATTITUDE -r 50 - -rc_input start -d /dev/ttyS2 - -linux_pwm_out start -mixer load /dev/pwm_out etc/mixers/quad_x.main.mix -logger start -t -b 200 -mavlink boot_complete diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index b64ecf176f44..ac1169772505 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -3,13 +3,14 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import # system_power not implemented param set CBRK_SUPPLY_CHK 894281 + +param set-default BAT1_V_DIV 5.7 + # broadcast to LAN # always keep current config param set SYS_AUTOCONFIG 0 @@ -49,7 +50,7 @@ gps start -d /dev/ttySC0 -i uart -p ubx -s #ist8310 start -X # Airspeed -ms4525_airspeed start -X +ms4525do start -X rc_input start -d /dev/ttyAMA0 diff --git a/posix-configs/rpi/pilotpi_mc.config b/posix-configs/rpi/pilotpi_mc.config index af447beddff4..6a9836d42562 100644 --- a/posix-configs/rpi/pilotpi_mc.config +++ b/posix-configs/rpi/pilotpi_mc.config @@ -3,13 +3,14 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import # system_power not implemented param set CBRK_SUPPLY_CHK 894281 + +param set-default BAT1_V_DIV 5.7 + # broadcast to LAN # always keep current config param set SYS_AUTOCONFIG 0 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 21059f6521cc..77e79cb20f68 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -5,10 +5,11 @@ # navio config for a quad -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import + +param set BAT1_V_DIV 10.177939394 +param set BAT1_A_PER_V 15.391030303 param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 1b4d096b0dea..28f1c4ce7ebf 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -5,10 +5,11 @@ # navio config for FW -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import + +param set BAT1_V_DIV 10.177939394 +param set BAT1_A_PER_V 15.391030303 param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 2100 param set MAV_TYPE 1 @@ -32,7 +33,7 @@ board_adc start battery_status start gps start -d /dev/spidev0.0 -i spi -p ubx -ms4525_airspeed start -X +ms4525do start -X rc_update start sensors start commander start diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 091e09063f05..3d769670cc07 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -7,10 +7,9 @@ # connect to it with jMAVSim: # ./Tools/jmavsim_run.sh -q -i -p 14577 -r 250 -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import + param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 1001 param set MAV_TYPE 2 diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index dc07b49396a7..3fa50aa00d75 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -5,10 +5,9 @@ # navio config for simple testing -if [ -f eeprom/parameters ] -then - param load -fi +param select eeprom/parameters +param import + param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 diff --git a/src/.gitignore b/src/.gitignore deleted file mode 100644 index 0ee2771b778d..000000000000 --- a/src/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ - -/generated_serial_params.c - diff --git a/src/drivers/Kconfig b/src/drivers/Kconfig new file mode 100644 index 000000000000..6353836988b2 --- /dev/null +++ b/src/drivers/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/drivers/adc/Kconfig b/src/drivers/adc/Kconfig new file mode 100644 index 000000000000..9dd780b3e838 --- /dev/null +++ b/src/drivers/adc/Kconfig @@ -0,0 +1,3 @@ +menu "ADC" +rsource "*/Kconfig" +endmenu #ADC diff --git a/src/drivers/adc/ads1115/ADS1115.cpp b/src/drivers/adc/ads1115/ADS1115.cpp index a54fb8b91de4..1e36c3757071 100644 --- a/src/drivers/adc/ads1115/ADS1115.cpp +++ b/src/drivers/adc/ads1115/ADS1115.cpp @@ -47,7 +47,18 @@ int ADS1115::init() config[0] = CONFIG_HIGH_OS_NOACT | CONFIG_HIGH_MUX_P0NG | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; config[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; - return writeReg(ADDRESSPOINTER_REG_CONFIG, config, 2); + ret = writeReg(ADDRESSPOINTER_REG_CONFIG, config, 2); + + if (ret != PX4_OK) { + PX4_ERR("writeReg failed (%i)", ret); + return ret; + } + + setChannel(ADS1115::A0); // prepare for the first measure. + + ScheduleOnInterval(SAMPLE_INTERVAL / 4, SAMPLE_INTERVAL / 4); + + return PX4_OK; } int ADS1115::setChannel(ADS1115::ChannelSelection ch) @@ -194,4 +205,4 @@ int ADS1115::writeReg(uint8_t addr, uint8_t *buf, size_t len) buffer[0] = addr; memcpy(buffer + 1, buf, sizeof(uint8_t)*len); return transfer(buffer, len + 1, nullptr, 0); -} \ No newline at end of file +} diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 9681c8ef26cf..ca1aa194c703 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -105,24 +105,17 @@ using namespace time_literals; class ADS1115 : public device::I2C, public I2CSPIDriver { public: - ADS1115(I2CSPIBusOption bus_option, int bus, int addr, int bus_frequency); + ADS1115(const I2CSPIDriverConfig &config); ~ADS1115() override; - int Begin(); - int init() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); void RunImpl(); protected: - adc_report_s _adc_report = {}; - void print_status() override; void exit_and_cleanup() override; @@ -133,9 +126,11 @@ class ADS1115 : public device::I2C, public I2CSPIDriver static const hrt_abstime SAMPLE_INTERVAL{50_ms}; + adc_report_s _adc_report{}; + perf_counter_t _cycle_perf; - int _channel_cycle_count = 0; + int _channel_cycle_count{0}; // ADS1115 logic part enum ChannelSelection { @@ -159,4 +154,4 @@ class ADS1115 : public device::I2C, public I2CSPIDriver int writeReg(uint8_t addr, uint8_t *buf, size_t len); -}; \ No newline at end of file +}; diff --git a/src/drivers/adc/ads1115/Kconfig b/src/drivers/adc/ads1115/Kconfig new file mode 100644 index 000000000000..c9f1741d951e --- /dev/null +++ b/src/drivers/adc/ads1115/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ADC_ADS1115 + bool "ads1115" + default n + ---help--- + Enable support for ads1115 diff --git a/src/drivers/adc/ads1115/ads1115_main.cpp b/src/drivers/adc/ads1115/ads1115_main.cpp index 9d09f09e9508..110ccda800ff 100644 --- a/src/drivers/adc/ads1115/ads1115_main.cpp +++ b/src/drivers/adc/ads1115/ads1115_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2020, 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 @@ -42,9 +42,9 @@ #include #include -ADS1115::ADS1115(I2CSPIBusOption bus_option, int bus, int addr, int bus_frequency) : - I2C(DRV_ADC_DEVTYPE_ADS1115, nullptr, bus, addr, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, addr), +ADS1115::ADS1115(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")) { _adc_report.device_id = this->get_device_id(); @@ -63,22 +63,6 @@ ADS1115::~ADS1115() perf_free(_cycle_perf); } -int ADS1115::Begin() -{ - int ret = init(); - - if (ret != PX4_OK) { - PX4_ERR("ADS1115 init failed"); - return ret; - } - - setChannel(ADS1115::A0); // prepare for the first measure. - - ScheduleOnInterval(SAMPLE_INTERVAL / 4, SAMPLE_INTERVAL / 4); - - return PX4_OK; -} - void ADS1115::exit_and_cleanup() { I2CSPIDriverBase::exit_and_cleanup(); // nothing to do @@ -140,25 +124,6 @@ void ADS1115::RunImpl() perf_end(_cycle_perf); } -I2CSPIDriverBase *ADS1115::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ADS1115 *instance = new ADS1115(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, - cli.bus_frequency); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->Begin()) { - delete instance; - return nullptr; - } - - return instance; -} - void ADS1115::print_usage() { PRINT_MODULE_USAGE_NAME("ads1115", "driver"); @@ -176,17 +141,12 @@ void ADS1115::print_status() extern "C" int ads1115_main(int argc, char *argv[]) { - int ch; using ThisDriver = ADS1115; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 400000; cli.i2c_address = 0x48; - while ((ch = cli.getopt(argc, argv, "")) != EOF) { - - } - - const char *verb = cli.optarg(); + const char *verb = cli.parseDefaultArguments(argc, argv); if (!verb) { ThisDriver::print_usage(); @@ -209,4 +169,4 @@ extern "C" int ads1115_main(int argc, char *argv[]) ThisDriver::print_usage(); return -1; -} \ No newline at end of file +} diff --git a/src/drivers/adc/ads1115/ads1115_params.c b/src/drivers/adc/ads1115/ads1115_params.c new file mode 100644 index 000000000000..560637db00a7 --- /dev/null +++ b/src/drivers/adc/ads1115/ads1115_params.c @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Enable external ADS1115 ADC + * + * If enabled, the internal ADC is not used. + * + * @boolean + * @reboot_required true + * @group Sensors + */ +PARAM_DEFINE_INT32(ADC_ADS1115_EN, 0); + diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp index 7552dd8a20ae..cccd64ff96cf 100644 --- a/src/drivers/adc/board_adc/ADC.cpp +++ b/src/drivers/adc/board_adc/ADC.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-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 @@ -55,7 +55,7 @@ ADC::ADC(uint32_t base_address, uint32_t channels) : } if (_channel_count > PX4_MAX_ADC_CHANNELS) { - PX4_ERR("PX4_MAX_ADC_CHANNELS is too small (%d, %d)", (unsigned)PX4_MAX_ADC_CHANNELS, _channel_count); + PX4_ERR("PX4_MAX_ADC_CHANNELS is too small (%u, %u)", PX4_MAX_ADC_CHANNELS, _channel_count); } _samples = new px4_adc_msg_t[_channel_count]; @@ -307,14 +307,14 @@ int ADC::test() px4_usleep(20000); // sleep 20ms and wait for adc report if (adc_sub_test.update(&adc)) { - PX4_INFO_RAW("DeviceID: %d\n", adc.device_id); - PX4_INFO_RAW("Resolution: %d\n", adc.resolution); + PX4_INFO_RAW("DeviceID: %" PRId32 "\n", adc.device_id); + PX4_INFO_RAW("Resolution: %" PRId32 "\n", adc.resolution); PX4_INFO_RAW("Voltage Reference: %f\n", (double)adc.v_ref); for (unsigned l = 0; l < 20; ++l) { for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; ++i) { if (adc.channel_id[i] >= 0) { - PX4_INFO_RAW("% 2d:% 6d", adc.channel_id[i], adc.raw_data[i]); + PX4_INFO_RAW("% 2" PRId16 " :% 6" PRId32, adc.channel_id[i], adc.raw_data[i]); } } diff --git a/src/drivers/adc/board_adc/ADC.hpp b/src/drivers/adc/board_adc/ADC.hpp index b53fe3d47edc..ab78fe332930 100644 --- a/src/drivers/adc/board_adc/ADC.hpp +++ b/src/drivers/adc/board_adc/ADC.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-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 @@ -37,6 +37,7 @@ * Driver for an ADC. * */ +#include #include #include diff --git a/src/drivers/adc/board_adc/Kconfig b/src/drivers/adc/board_adc/Kconfig new file mode 100644 index 000000000000..d42ac3a5ee26 --- /dev/null +++ b/src/drivers/adc/board_adc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ADC_BOARD_ADC + bool "board_adc" + default n + ---help--- + Enable support for board_adc \ No newline at end of file diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index b7ec8312f682..ba90108c38c2 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -38,5 +38,8 @@ add_subdirectory(lps22hb) #add_subdirectory(lps25h) # not ready for general inclusion add_subdirectory(lps33hw) #add_subdirectory(mpl3115a2) # not ready for general inclusion +add_subdirectory(maiertek) add_subdirectory(ms5611) #add_subdirectory(tcbp001ta) # only for users who really need this +add_subdirectory(invensense) +add_subdirectory(goertek) diff --git a/src/drivers/barometer/Kconfig b/src/drivers/barometer/Kconfig new file mode 100644 index 000000000000..f99c053cf67c --- /dev/null +++ b/src/drivers/barometer/Kconfig @@ -0,0 +1,16 @@ +menu "barometer" + menuconfig COMMON_BAROMETERS + bool "Common barometers" + default n + select DRIVERS_BAROMETER_BMP280 + select DRIVERS_BAROMETER_BMP388 + select DRIVERS_BAROMETER_DPS310 + select DRIVERS_BAROMETER_LPS22HB + select DRIVERS_BAROMETER_LPS33HW + select DRIVERS_BAROMETER_MS5611 + select DRIVERS_BAROMETER_MAIERTEK_MPC2520 + select DRIVERS_BAROMETER_GOERTEK_SPL06 + ---help--- + Enable default set of barometer drivers + rsource "*/Kconfig" +endmenu #barometer diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index dc2fc51ed652..ae5387ad7c2a 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -33,10 +33,8 @@ #include "BMP280.hpp" -BMP280::BMP280(I2CSPIBusOption bus_option, int bus, bmp280::IBMP280 *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, - interface->get_device_address()), - _px4_baro(interface->get_device_id()), +BMP280::BMP280(const I2CSPIDriverConfig &config, bmp280::IBMP280 *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), @@ -173,11 +171,15 @@ BMP280::collect() float pf = ((float) p_raw + x1) / x2; const float P = (pf * _fcal.p9 + _fcal.p8) * pf + _fcal.p7; - _px4_baro.set_error_count(perf_event_count(_comms_errors)); - _px4_baro.set_temperature(T); - - float pressure = P / 100.0f; // to mbar - _px4_baro.update(timestamp_sample, pressure); + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = P; + sensor_baro.temperature = T; + 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); diff --git a/src/drivers/barometer/bmp280/BMP280.hpp b/src/drivers/barometer/bmp280/BMP280.hpp index 3759062801df..bb5aa28f833f 100644 --- a/src/drivers/barometer/bmp280/BMP280.hpp +++ b/src/drivers/barometer/bmp280/BMP280.hpp @@ -39,17 +39,17 @@ #include #include #include -#include +#include +#include #include class BMP280 : public I2CSPIDriver { public: - BMP280(I2CSPIBusOption bus_option, int bus, bmp280::IBMP280 *interface); + BMP280(const I2CSPIDriverConfig &config, bmp280::IBMP280 *interface); virtual ~BMP280(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -62,7 +62,7 @@ class BMP280 : public I2CSPIDriver int measure(); //start measure int collect(); //get results and publish - PX4Barometer _px4_baro; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; bmp280::IBMP280 *_interface; diff --git a/src/drivers/barometer/bmp280/BMP280_I2C.cpp b/src/drivers/barometer/bmp280/BMP280_I2C.cpp index 71e516ce6167..1691f8f53a52 100644 --- a/src/drivers/barometer/bmp280/BMP280_I2C.cpp +++ b/src/drivers/barometer/bmp280/BMP280_I2C.cpp @@ -42,6 +42,7 @@ #include #include +#if defined(CONFIG_I2C) class BMP280_I2C: public device::I2C, public bmp280::IBMP280 { @@ -116,3 +117,5 @@ BMP280_I2C::get_calibration(uint8_t addr) return nullptr; } } + +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/bmp280/BMP280_SPI.cpp b/src/drivers/barometer/bmp280/BMP280_SPI.cpp index 0bc4f1d9d3a0..9ab79b589f69 100644 --- a/src/drivers/barometer/bmp280/BMP280_SPI.cpp +++ b/src/drivers/barometer/bmp280/BMP280_SPI.cpp @@ -42,6 +42,8 @@ #include #include +#if defined(CONFIG_SPI) + /* SPI protocol address bits */ #define DIR_READ (1<<7) //for set #define DIR_WRITE ~(1<<7) //for clear @@ -132,3 +134,4 @@ BMP280_SPI::get_calibration(uint8_t addr) return nullptr; } } +#endif // CONFIG_SPI diff --git a/src/drivers/barometer/bmp280/CMakeLists.txt b/src/drivers/barometer/bmp280/CMakeLists.txt index 7bdb1ea18a9d..8d53de6231bd 100644 --- a/src/drivers/barometer/bmp280/CMakeLists.txt +++ b/src/drivers/barometer/bmp280/CMakeLists.txt @@ -42,6 +42,5 @@ px4_add_module( bmp280_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp280/Kconfig b/src/drivers/barometer/bmp280/Kconfig new file mode 100644 index 000000000000..25b6fed476a6 --- /dev/null +++ b/src/drivers/barometer/bmp280/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_BMP280 + bool "bmp280" + default n + ---help--- + Enable support for bmp280 \ No newline at end of file diff --git a/src/drivers/barometer/bmp280/bmp280.h b/src/drivers/barometer/bmp280/bmp280.h index 9103c9797318..bcdf024a66b9 100644 --- a/src/drivers/barometer/bmp280/bmp280.h +++ b/src/drivers/barometer/bmp280/bmp280.h @@ -38,8 +38,7 @@ */ #pragma once -#include -#include +#include #define BMP280_ADDR_CAL 0x88 /* address of 12x 2 bytes calibration data */ #define BMP280_ADDR_DATA 0xF7 /* address of 2x 3 bytes p-t data */ @@ -159,5 +158,9 @@ class IBMP280 /* interface factories */ +#if defined(CONFIG_SPI) extern bmp280::IBMP280 *bmp280_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) extern bmp280::IBMP280 *bmp280_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency); +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/bmp280/bmp280_main.cpp b/src/drivers/barometer/bmp280/bmp280_main.cpp index a77da14357e0..da5639e43f24 100644 --- a/src/drivers/barometer/bmp280/bmp280_main.cpp +++ b/src/drivers/barometer/bmp280/bmp280_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019, 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 @@ -37,6 +37,8 @@ #include "BMP280.hpp" +#include + extern "C" { __EXPORT int bmp280_main(int argc, char *argv[]); } void @@ -45,35 +47,47 @@ BMP280::print_usage() PRINT_MODULE_USAGE_NAME("bmp280", "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 *BMP280::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *BMP280::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { bmp280::IBMP280 *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = bmp280_i2c_interface(iterator.bus(), cli.i2c_address, cli.bus_frequency); +#if defined(CONFIG_I2C) + + if (config.bus_type == BOARD_I2C_BUS) { + interface = bmp280_i2c_interface(config.bus, config.i2c_address, config.bus_frequency); + + } + +#endif // CONFIG_I2C +#if defined(CONFIG_SPI) - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = bmp280_spi_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + if (config.bus_type == BOARD_SPI_BUS) { + interface = bmp280_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 (devid 0x%x)", iterator.bus(), iterator.devid()); + 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 (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i", config.bus); return nullptr; } - BMP280 *dev = new BMP280(iterator.configuredBusOption(), iterator.bus(), interface); + BMP280 *dev = new BMP280(config, interface); if (dev == nullptr) { delete interface; @@ -93,9 +107,13 @@ bmp280_main(int argc, char *argv[]) { using ThisDriver = BMP280; 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); diff --git a/src/drivers/barometer/bmp388/CMakeLists.txt b/src/drivers/barometer/bmp388/CMakeLists.txt index 35474254de7a..0b4cac9e509a 100644 --- a/src/drivers/barometer/bmp388/CMakeLists.txt +++ b/src/drivers/barometer/bmp388/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( bmp388.cpp bmp388_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/bmp388/Kconfig b/src/drivers/barometer/bmp388/Kconfig new file mode 100644 index 000000000000..585c4e975e06 --- /dev/null +++ b/src/drivers/barometer/bmp388/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_BMP388 + bool "bmp388" + default n + ---help--- + Enable support for bmp388 \ No newline at end of file diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index 8a28ecdbf805..ebfa5f9b52ed 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/src/drivers/barometer/bmp388/bmp388.cpp @@ -41,10 +41,8 @@ #include "bmp388.h" -BMP388::BMP388(I2CSPIBusOption bus_option, int bus, IBMP388 *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, - interface->get_device_address()), - _px4_baro(interface->get_device_id()), +BMP388::BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), @@ -166,14 +164,18 @@ BMP388::collect() return -EIO; } - _px4_baro.set_error_count(perf_event_count(_comms_errors)); - float temperature = (float)(data.temperature / 100.0f); float pressure = (float)(data.pressure / 100.0f); // to Pascal - pressure = pressure / 100.0f; // to mbar - _px4_baro.set_temperature(temperature); - _px4_baro.update(timestamp_sample, pressure); + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = pressure; + 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); diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h index 295bae374b13..1fd5f4a41875 100644 --- a/src/drivers/barometer/bmp388/bmp388.h +++ b/src/drivers/barometer/bmp388/bmp388.h @@ -44,7 +44,8 @@ #include #include #include -#include +#include +#include #include "board_config.h" @@ -308,11 +309,10 @@ class IBMP388 class BMP388 : public I2CSPIDriver { public: - BMP388(I2CSPIBusOption bus_option, int bus, IBMP388 *interface); + BMP388(const I2CSPIDriverConfig &config, IBMP388 *interface); virtual ~BMP388(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual int init(); @@ -326,7 +326,7 @@ class BMP388 : public I2CSPIDriver static constexpr uint8_t odr{BMP3_ODR_50_HZ}; // output data rate (not used) static constexpr uint8_t iir_coef{BMP3_IIR_FILTER_DISABLE}; // IIR coefficient - PX4Barometer _px4_baro; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; IBMP388 *_interface{nullptr}; unsigned _measure_interval{0}; // interval in microseconds needed to measure diff --git a/src/drivers/barometer/bmp388/bmp388_main.cpp b/src/drivers/barometer/bmp388/bmp388_main.cpp index 45d4097d5ee4..ff32ee206870 100644 --- a/src/drivers/barometer/bmp388/bmp388_main.cpp +++ b/src/drivers/barometer/bmp388/bmp388_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -48,30 +48,29 @@ BMP388::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *BMP388::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *BMP388::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { IBMP388 *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = bmp388_i2c_interface(iterator.bus(), cli.i2c_address, cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = bmp388_i2c_interface(config.bus, config.i2c_address, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = bmp388_spi_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = bmp388_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } - BMP388 *dev = new BMP388(iterator.configuredBusOption(), iterator.bus(), interface); + BMP388 *dev = new BMP388(config, interface); if (dev == nullptr) { delete interface; diff --git a/src/drivers/barometer/dps310/CMakeLists.txt b/src/drivers/barometer/dps310/CMakeLists.txt index 088e56e6b581..8227591962f0 100644 --- a/src/drivers/barometer/dps310/CMakeLists.txt +++ b/src/drivers/barometer/dps310/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( DPS310_SPI.cpp dps310_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/dps310/DPS310.cpp b/src/drivers/barometer/dps310/DPS310.cpp index 3aba3feb0bc0..a9e1f73a0d78 100644 --- a/src/drivers/barometer/dps310/DPS310.cpp +++ b/src/drivers/barometer/dps310/DPS310.cpp @@ -46,10 +46,8 @@ static void getTwosComplement(T &raw, uint8_t length) } } -DPS310::DPS310(I2CSPIBusOption bus_option, int bus, device::Device *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, - interface->get_device_address()), - _px4_barometer(interface->get_device_id()), +DPS310::DPS310(const I2CSPIDriverConfig &config, device::Device *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comm errors")) @@ -234,9 +232,15 @@ DPS310::RunImpl() const float Tcomp = c0 * 0.5f + c1 * Traw_sc; - _px4_barometer.set_error_count(perf_event_count(_comms_errors)); - _px4_barometer.set_temperature(Tcomp); - _px4_barometer.update(timestamp_sample, Pcomp / 100.0f); // Pascals -> Millibar + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = Pcomp; + sensor_baro.temperature = Tcomp; + 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); } diff --git a/src/drivers/barometer/dps310/DPS310.hpp b/src/drivers/barometer/dps310/DPS310.hpp index 74395d68e76d..06c74a9e0535 100644 --- a/src/drivers/barometer/dps310/DPS310.hpp +++ b/src/drivers/barometer/dps310/DPS310.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -40,7 +40,8 @@ #pragma once #include -#include +#include +#include #include #include #include @@ -56,11 +57,10 @@ using Infineon_DPS310::Register; class DPS310 : public I2CSPIDriver { public: - DPS310(I2CSPIBusOption bus_option, int bus, device::Device *interface); + DPS310(const I2CSPIDriverConfig &config, device::Device *interface); virtual ~DPS310(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -80,7 +80,7 @@ class DPS310 : public I2CSPIDriver static constexpr uint32_t SAMPLE_RATE{32}; - PX4Barometer _px4_barometer; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; device::Device *_interface; diff --git a/src/drivers/barometer/dps310/DPS310_I2C.cpp b/src/drivers/barometer/dps310/DPS310_I2C.cpp index 55c2acb2bdcd..7deec9f0a23c 100644 --- a/src/drivers/barometer/dps310/DPS310_I2C.cpp +++ b/src/drivers/barometer/dps310/DPS310_I2C.cpp @@ -39,6 +39,8 @@ #include +#if defined(CONFIG_I2C) + namespace dps310 { @@ -89,3 +91,5 @@ DPS310_I2C::write(unsigned address, void *data, unsigned count) } } // namespace dps310 + +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/dps310/Kconfig b/src/drivers/barometer/dps310/Kconfig new file mode 100644 index 000000000000..efd7746b0985 --- /dev/null +++ b/src/drivers/barometer/dps310/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_DPS310 + bool "dps310" + default n + ---help--- + Enable support for dps310 \ No newline at end of file diff --git a/src/drivers/barometer/dps310/dps310_main.cpp b/src/drivers/barometer/dps310/dps310_main.cpp index 02a30a303066..57e5a36ea16a 100644 --- a/src/drivers/barometer/dps310/dps310_main.cpp +++ b/src/drivers/barometer/dps310/dps310_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -36,7 +36,9 @@ namespace dps310 { extern device::Device *DPS310_SPI_interface(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +#if defined(CONFIG_I2C) extern device::Device *DPS310_I2C_interface(uint8_t bus, uint32_t device, int bus_frequency); +#endif // CONFIG_I2C } #include @@ -50,35 +52,42 @@ DPS310::print_usage() PRINT_MODULE_USAGE_NAME("dps310", "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(0x77); +#else + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); +#endif PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *DPS310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *DPS310::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = DPS310_I2C_interface(iterator.bus(), cli.i2c_address, cli.bus_frequency); +#if defined(CONFIG_I2C) - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = DPS310_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); - } + if (config.bus_type == BOARD_I2C_BUS) { + interface = DPS310_I2C_interface(config.bus, config.i2c_address, config.bus_frequency); + + } else +#endif // CONFIG_I2C + if (config.bus_type == BOARD_SPI_BUS) { + interface = DPS310_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + } if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } - DPS310 *dev = new DPS310(iterator.configuredBusOption(), iterator.bus(), interface); + DPS310 *dev = new DPS310(config, interface); if (dev == nullptr) { delete interface; @@ -96,9 +105,15 @@ I2CSPIDriverBase *DPS310::instantiate(const BusCLIArguments &cli, const BusInsta extern "C" int dps310_main(int argc, char *argv[]) { using ThisDriver = DPS310; - BusCLIArguments cli{true, true}; + +#if defined(CONFIG_I2C) + BusCLIArguments cli {true, true}; cli.i2c_address = 0x77; cli.default_i2c_frequency = 400000; +#else + BusCLIArguments cli {false, true}; +#endif // CONFIG_I2C + cli.default_spi_frequency = 10 * 1000 * 1000; const char *verb = cli.parseDefaultArguments(argc, argv); diff --git a/src/drivers/barometer/goertek/CMakeLists.txt b/src/drivers/barometer/goertek/CMakeLists.txt new file mode 100644 index 000000000000..867eb64fcaa0 --- /dev/null +++ b/src/drivers/barometer/goertek/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(spl06) diff --git a/src/drivers/barometer/goertek/Kconfig b/src/drivers/barometer/goertek/Kconfig new file mode 100644 index 000000000000..a82b5a47e15e --- /dev/null +++ b/src/drivers/barometer/goertek/Kconfig @@ -0,0 +1,3 @@ +menu "Goertek" +rsource "*/Kconfig" +endmenu #Goertek diff --git a/src/drivers/barometer/goertek/spl06/CMakeLists.txt b/src/drivers/barometer/goertek/spl06/CMakeLists.txt new file mode 100644 index 000000000000..044ca466a52d --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__barometer__spl06 + MAIN spl06 + SRCS + SPL06.cpp + SPL06.hpp + SPL06_I2C.cpp + SPL06_SPI.cpp + spl06_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/goertek/spl06/Kconfig b/src/drivers/barometer/goertek/spl06/Kconfig new file mode 100644 index 000000000000..2579437d0b38 --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_GOERTEK_SPL06 + bool "spl06" + default n + ---help--- + Enable support for spl06 diff --git a/src/drivers/barometer/goertek/spl06/SPL06.cpp b/src/drivers/barometer/goertek/spl06/SPL06.cpp new file mode 100644 index 000000000000..9179996b321c --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/SPL06.cpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * 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 "SPL06.hpp" + +SPL06::SPL06(const I2CSPIDriverConfig &config, spl06::ISPL06 *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")) +{ +} + +SPL06::~SPL06() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + + delete _interface; +} + +/* +float +SPL06::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 +SPL06::calibrate() +{ + uint8_t buf[18]; + + _interface->read(SPL06_ADDR_CAL, buf, sizeof(buf)); + + _cal.c0 = (uint16_t)buf[0] << 4 | (uint16_t)buf[1] >> 4; + _cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0; + + _cal.c1 = (uint16_t)(buf[1] & 0x0f) << 8 | (uint16_t)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 | (uint16_t)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 | (uint32_t)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]; + + // PX4_INFO("c0:%d \nc1:%d \nc00:%d \nc10:%d \nc01:%d \nc11:%d \nc20:%d \nc21:%d \nc30:%d\n", + // _cal.c0,_cal.c1, + // _cal.c00,_cal.c10, + // _cal.c01,_cal.c11,_cal.c20,_cal.c21,_cal.c30 + // ); + //PX4_DEBUG("c0:%f",_cal.c0); + return OK; +} +int +SPL06::init() +{ + int8_t tries = 5; + // reset sensor + _interface->set_reg(SPL06_VALUE_RESET, SPL06_ADDR_RESET); + usleep(10000); + + // check id + if (_interface->get_reg(SPL06_ADDR_ID) != SPL06_VALUE_ID) { + PX4_DEBUG("id of your baro is not: 0x%02x", SPL06_VALUE_ID); + return -EIO; + } + + while (tries--) { + uint8_t meas_cfg = _interface->get_reg(SPL06_ADDR_MEAS_CFG); + + if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) { + break; + } + + usleep(10000); + } + + if (tries < 0) { + PX4_DEBUG("spl06 cal failed"); + return -EIO; + } + + // get calibration and pre process them + calibrate(); + + // set config, recommended settings + _interface->set_reg(_curr_prs_cfg, SPL06_ADDR_PRS_CFG); + kp = 253952.0f; // refer to scale_factor() + _interface->set_reg(_curr_tmp_cfg, SPL06_ADDR_TMP_CFG); + kt = 524288.0f; + + + _interface->set_reg(1 << 2, SPL06_ADDR_CFG_REG); + _interface->set_reg(7, SPL06_ADDR_MEAS_CFG); + + Start(); + + return OK; +} + +void +SPL06::Start() +{ + // schedule a cycle to start things + ScheduleNow(); +} + +void +SPL06::RunImpl() +{ + collect(); + + ScheduleDelayed(_measure_interval); +} +int +SPL06::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(SPL06_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); + float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * (float)_cal.c21); + + 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 +SPL06::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/spl06/SPL06.hpp b/src/drivers/barometer/goertek/spl06/SPL06.hpp new file mode 100644 index 000000000000..e62deb937127 --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/SPL06.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "spl06.h" + +#include +#include +#include +#include +#include +#include +#include + +class SPL06 : public I2CSPIDriver +{ +public: + SPL06(const I2CSPIDriverConfig &config, spl06::ISPL06 *interface); + virtual ~SPL06(); + + 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)}; + + spl06::ISPL06 *_interface; + spl06::data_s _data; + spl06::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) + // + // bit[7]: reserved + // + // PM_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // 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 + static constexpr uint8_t _curr_prs_cfg{4 << 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) + // TMP_EXT[7] : 0 | 1 + // note: it is highly recommended to use the same temperature sensor as the source of the calibration coefficients wihch can be read from reg 0x28 + // + // TMP_RATE[6:4] : 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 + // measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 + // note: applicable for measurements in background mode only + // + // bit[3]: reserved + // + // 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 + static constexpr uint8_t _curr_tmp_cfg{1 << 7 | 4 << 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{16}; + static constexpr uint32_t _measure_interval{1000000 / _sample_rate / 2}; +}; diff --git a/src/drivers/barometer/goertek/spl06/SPL06_I2C.cpp b/src/drivers/barometer/goertek/spl06/SPL06_I2C.cpp new file mode 100644 index 000000000000..26c7443b573c --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/SPL06_I2C.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 spl06_i2c.cpp + * + * SPI interface for Goertek SPL06 + */ + +#include "spl06.h" + +#include +#include + +#if defined(CONFIG_I2C) + +class SPL06_I2C: public device::I2C, public spl06::ISPL06 +{ +public: + SPL06_I2C(uint8_t bus, uint32_t device, int bus_frequency); + virtual ~SPL06_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; + //spl06::data_s *get_data(uint8_t addr) override; + //spl06::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: + spl06::calibration_s _cal{}; + spl06::data_s _data{}; +}; + +spl06::ISPL06 *spl06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency) +{ + return new SPL06_I2C(busnum, device, bus_frequency); +} + +SPL06_I2C::SPL06_I2C(uint8_t bus, uint32_t device, int bus_frequency) : + I2C(DRV_BARO_DEVTYPE_SPL06, MODULE_NAME, bus, device, bus_frequency) +{ +} + +uint8_t +SPL06_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 +SPL06_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 +SPL06_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/spl06/SPL06_SPI.cpp b/src/drivers/barometer/goertek/spl06/SPL06_SPI.cpp new file mode 100644 index 000000000000..3c39ddbe4926 --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/SPL06_SPI.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 spl06_spi.cpp + * + * SPI interface for Goertek SPL06 + */ + +#include "spl06.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 SPL06_SPI: public device::SPI, public spl06::ISPL06 +{ +public: + SPL06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~SPL06_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(); } +}; + +spl06::ISPL06 * +spl06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode) +{ + return new SPL06_SPI(busnum, device, bus_frequency, spi_mode); +} + +SPL06_SPI::SPL06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_SPL06, MODULE_NAME, bus, device, spi_mode, bus_frequency) +{ +} + +uint8_t +SPL06_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 +SPL06_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 +SPL06_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/spl06/parameters.c b/src/drivers/barometer/goertek/spl06/parameters.c new file mode 100644 index 000000000000..2712844477a2 --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Goertek SPL06 Barometer (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SPL06, 0); diff --git a/src/drivers/barometer/goertek/spl06/spl06.h b/src/drivers/barometer/goertek/spl06/spl06.h new file mode 100644 index 000000000000..06f37baf73be --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/spl06.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 spl06.h + * + * Shared defines for the spl06 driver. + */ +#pragma once + +#include + +#define SPL06_ADDR_ID 0x0d +#define SPL06_ADDR_RESET 0x0c // set to reset +#define SPL06_ADDR_CAL 0x10 +#define SPL06_ADDR_PRS_CFG 0x06 +#define SPL06_ADDR_TMP_CFG 0x07 +#define SPL06_ADDR_MEAS_CFG 0x08 +#define SPL06_ADDR_CFG_REG 0x09 +#define SPL06_ADDR_DATA 0x00 + + +#define SPL06_VALUE_RESET 9 +#define SPL06_VALUE_ID 0x10 + +namespace spl06 +{ + +#pragma pack(push,1) +struct calibration_s { + int16_t c0, c1; + int32_t c00, c10; + int16_t c01, c11, c20, c21, c30; +}; //calibration data + +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; +}; // data +#pragma pack(pop) + +class ISPL06 +{ +public: + virtual ~ISPL06() = 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 */ + + +/* interface factories */ +#if defined(CONFIG_SPI) +extern spl06::ISPL06 *spl06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +#endif // CONFIG_SPI +#if defined(CONFIG_I2C) +extern spl06::ISPL06 *spl06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency); +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/goertek/spl06/spl06_main.cpp b/src/drivers/barometer/goertek/spl06/spl06_main.cpp new file mode 100644 index 000000000000..8da32b77d5d1 --- /dev/null +++ b/src/drivers/barometer/goertek/spl06/spl06_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 "SPL06.hpp" + +#include + +extern "C" { __EXPORT int spl06_main(int argc, char *argv[]); } + +void +SPL06::print_usage() +{ + PRINT_MODULE_USAGE_NAME("spl06", "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 *SPL06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + spl06::ISPL06 *interface = nullptr; + +#if defined(CONFIG_I2C) + + if (config.bus_type == BOARD_I2C_BUS) { + interface = spl06_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 = spl06_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; + } + + SPL06 *dev = new SPL06(config, interface); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +int +spl06_main(int argc, char *argv[]) +{ + using ThisDriver = SPL06; + 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_SPL06); + + 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/barometer/invensense/CMakeLists.txt b/src/drivers/barometer/invensense/CMakeLists.txt new file mode 100755 index 000000000000..1060e92e661c --- /dev/null +++ b/src/drivers/barometer/invensense/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(icp10100) +add_subdirectory(icp10111) diff --git a/src/drivers/barometer/invensense/icp10100/CMakeLists.txt b/src/drivers/barometer/invensense/icp10100/CMakeLists.txt new file mode 100755 index 000000000000..7b7ffadab830 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10100/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__invensense__icp10100 + MAIN icp10100 + COMPILE_FLAGS + SRCS + Inven_Sense_ICP10100_registers.hpp + ICP10100.cpp + ICP10100.hpp + icp10100_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/invensense/icp10100/ICP10100.cpp b/src/drivers/barometer/invensense/icp10100/ICP10100.cpp new file mode 100755 index 000000000000..437b05b64197 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10100/ICP10100.cpp @@ -0,0 +1,379 @@ +/**************************************************************************** + * + * 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 "ICP10100.hpp" + +using namespace time_literals; + +ICP10100::ICP10100(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ICP10100::~ICP10100() +{ + perf_free(_reset_perf); + perf_free(_sample_perf); + perf_free(_bad_transfer_perf); +} + +int +ICP10100::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool +ICP10100::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void +ICP10100::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_reset_perf); + perf_print_counter(_sample_perf); + perf_print_counter(_bad_transfer_perf); +} + +int +ICP10100::probe() +{ + uint16_t ID = 0; + read_response(Cmd::READ_ID, (uint8_t *)&ID, 2); + uint8_t PROD_ID = (ID >> 8) & 0x3f; + + if (PROD_ID != Product_ID) { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void +ICP10100::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // Software Reset + send_command(Cmd::SOFT_RESET); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(100_ms); // Power On Reset: max 100ms + break; + + case STATE::WAIT_FOR_RESET: { + // check product id + uint16_t ID = 0; + read_response(Cmd::READ_ID, (uint8_t *)&ID, 2); + uint8_t PROD_ID = (ID >> 8) & 0x3f; // Product ID Bits 5:0 + + if (PROD_ID == Product_ID) { + // if reset succeeded then read otp + _state = STATE::READ_OTP; + ScheduleDelayed(10_ms); // Time to coefficients are available. + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::READ_OTP: { + // read otp + uint8_t addr_otp_cmd[3] = {0x00, 0x66, 0x9c}; + uint8_t otp_buf[3]; + uint8_t crc; + bool success = true; + + send_command(Cmd::SET_ADDR, addr_otp_cmd, 3); + + for (uint8_t i = 0; i < 4; i++) { + read_response(Cmd::READ_OTP, otp_buf, 3); + + crc = 0xFF; + + for (int j = 0; j < 2; j++) { + crc = (uint8_t)cal_crc(crc, otp_buf[j]); + } + + if (crc != otp_buf[2]) { + success = false; + break; + } + + _scal[i] = (otp_buf[0] << 8) | otp_buf[1]; + } + + if (success) { + _state = STATE::MEASURE; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::MEASURE: + if (Measure()) { + // if configure succeeded then start measurement cycle + _state = STATE::READ; + perf_begin(_sample_perf); + ScheduleDelayed(_measure_interval); + + } else { + // MEASURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Measure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Measure failed, retrying"); + } + + ScheduleDelayed(_measure_interval); + } + + break; + + case STATE::READ: { + uint8_t comp_data[9] {}; + bool success = false; + + if (read_measure_results(comp_data, 9) == PX4_OK) { + perf_end(_sample_perf); + + uint16_t _raw_t = (comp_data[0] << 8) | comp_data[1]; + uint32_t L_res_buf3 = comp_data[3]; // expand result bytes to 32bit to fix issues on 8-bit MCUs + uint32_t L_res_buf4 = comp_data[4]; + uint32_t L_res_buf6 = comp_data[6]; + uint32_t _raw_p = (L_res_buf3 << 16) | (L_res_buf4 << 8) | L_res_buf6; + + // constants for presure calculation + static constexpr float _pcal[3] = { 45000.0, 80000.0, 105000.0 }; + static constexpr float _lut_lower = 3.5 * 0x100000; // 1<<20 + static constexpr float _lut_upper = 11.5 * 0x100000; // 1<<20 + static constexpr float _quadr_factor = 1 / 16777216.0; + static constexpr float _offst_factor = 2048.0; + + // calculate temperature + float _temperature_C = -45.f + 175.f / 65536.f * _raw_t; + + // calculate pressure + float t = (float)(_raw_t - 32768); + float s1 = _lut_lower + (float)(_scal[0] * t * t) * _quadr_factor; + float s2 = _offst_factor * _scal[3] + (float)(_scal[1] * t * t) * _quadr_factor; + float s3 = _lut_upper + (float)(_scal[2] * t * t) * _quadr_factor; + float c = (s1 * s2 * (_pcal[0] - _pcal[1]) + + s2 * s3 * (_pcal[1] - _pcal[2]) + + s3 * s1 * (_pcal[2] - _pcal[0])) / + (s3 * (_pcal[0] - _pcal[1]) + + s1 * (_pcal[1] - _pcal[2]) + + s2 * (_pcal[2] - _pcal[0])); + float a = (_pcal[0] * s1 - _pcal[1] * s2 - (_pcal[1] - _pcal[0]) * c) / (s1 - s2); + float b = (_pcal[0] - a) * (s1 + c); + float _pressure_Pa = a + b / (c + _raw_p); + + const hrt_abstime nowx = hrt_absolute_time(); + float temperature = _temperature_C; + float pressure = _pressure_Pa; + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = now; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = pressure; + sensor_baro.temperature = temperature; + sensor_baro.error_count = perf_event_count(_bad_transfer_perf); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + + _state = STATE::MEASURE; + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + ScheduleDelayed(1000_ms / 8 - _measure_interval); // 8Hz + } + + break; + } +} + +bool +ICP10100::Measure() +{ + /* + From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1 + + Sensor Measurement Max Time + Mode Time (Forced) + Low Power (LP) 1.6 ms 1.8 ms + Normal (N) 5.6 ms 6.3 ms + Low Noise (LN) 20.8 ms 23.8 ms + Ultra Low Noise(ULN) 83.2 ms 94.5 ms + */ + Cmd cmd; + + switch (_mode) { + case MODE::FAST: + cmd = Cmd::MEAS_LP; + _measure_interval = 2_ms; + break; + + case MODE::ACCURATE: + cmd = Cmd::MEAS_LN; + _measure_interval = 24_ms; + break; + + case MODE::VERY_ACCURATE: + cmd = Cmd::MEAS_ULN; + _measure_interval = 95_ms; + break; + + case MODE::NORMAL: + default: + cmd = Cmd::MEAS_N; + _measure_interval = 7_ms; + break; + } + + if (send_command(cmd) != PX4_OK) { + return false; + } + + return true; +} + +int8_t +ICP10100::cal_crc(uint8_t seed, uint8_t data) +{ + int8_t poly = 0x31; + int8_t var2; + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((seed & 0x80) ^ (data & 0x80)) { + var2 = 1; + + } else { + var2 = 0; + } + + seed = (seed & 0x7F) << 1; + data = (data & 0x7F) << 1; + seed = seed ^ (uint8_t)(poly * var2); + } + + return (int8_t)seed; +} + +int +ICP10100::read_measure_results(uint8_t *buf, uint8_t len) +{ + return transfer(nullptr, 0, buf, len); +} + +int +ICP10100::read_response(Cmd cmd, uint8_t *buf, uint8_t len) +{ + uint8_t buff[2]; + buff[0] = ((uint16_t)cmd >> 8) & 0xff; + buff[1] = (uint16_t)cmd & 0xff; + return transfer(&buff[0], 2, buf, len); +} + +int +ICP10100::send_command(Cmd cmd) +{ + uint8_t buf[2]; + buf[0] = ((uint16_t)cmd >> 8) & 0xff; + buf[1] = (uint16_t)cmd & 0xff; + return transfer(buf, sizeof(buf), nullptr, 0); +} + +int +ICP10100::send_command(Cmd cmd, uint8_t *data, uint8_t len) +{ + uint8_t buf[5]; + buf[0] = ((uint16_t)cmd >> 8) & 0xff; + buf[1] = (uint16_t)cmd & 0xff; + memcpy(&buf[2], data, len); + return transfer(&buf[0], len + 2, nullptr, 0); +} diff --git a/src/drivers/barometer/invensense/icp10100/ICP10100.hpp b/src/drivers/barometer/invensense/icp10100/ICP10100.hpp new file mode 100755 index 000000000000..ba887cd5e4e9 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10100/ICP10100.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 + +#include "Inven_Sense_ICP10100_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace Inven_Sense_ICP10100; + +class ICP10100 : public device::I2C, public I2CSPIDriver +{ +public: + ICP10100(const I2CSPIDriverConfig &config); + ~ICP10100() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + int probe() override; + + bool Reset(); + + bool Measure(); + + int8_t cal_crc(uint8_t seed, uint8_t data); + int read_measure_results(uint8_t *buf, uint8_t len); + int read_response(Cmd cmd, uint8_t *buf, uint8_t len); + int send_command(Cmd cmd); + int send_command(Cmd cmd, uint8_t *data, uint8_t len); + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + + hrt_abstime _reset_timestamp{0}; + int _failure_count{0}; + + unsigned _measure_interval{0}; + int16_t _scal[4]; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + READ_OTP, + MEASURE, + READ + } _state{STATE::RESET}; + + enum class MODE : uint8_t { + FAST, + NORMAL, + ACCURATE, + VERY_ACCURATE + } _mode{MODE::VERY_ACCURATE}; +}; diff --git a/src/drivers/barometer/invensense/icp10100/Inven_Sense_ICP10100_registers.hpp b/src/drivers/barometer/invensense/icp10100/Inven_Sense_ICP10100_registers.hpp new file mode 100755 index 000000000000..558b7f14d1b7 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10100/Inven_Sense_ICP10100_registers.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 icp10100_registers.hpp + * + * icp10100 registers. + * + */ + +#pragma once + +#include + +namespace Inven_Sense_ICP10100 +{ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x63; + +static constexpr uint8_t Product_ID = 0x08; + +enum class Cmd : uint16_t { + READ_ID = 0xefc8, + SET_ADDR = 0xc595, + READ_OTP = 0xc7f7, + MEAS_LP = 0x609c, + MEAS_N = 0x6825, + MEAS_LN = 0x70df, + MEAS_ULN = 0x7866, + SOFT_RESET = 0x805d +}; +} // namespace Inven_Sense_ICP10100 diff --git a/src/drivers/barometer/invensense/icp10100/icp10100_main.cpp b/src/drivers/barometer/invensense/icp10100/icp10100_main.cpp new file mode 100755 index 000000000000..de4c0aa25e5a --- /dev/null +++ b/src/drivers/barometer/invensense/icp10100/icp10100_main.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 + +#include "ICP10100.hpp" + +void +ICP10100::print_usage() +{ + PRINT_MODULE_USAGE_NAME("icp10100", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x63); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int icp10100_main(int argc, char *argv[]) +{ + using ThisDriver = ICP10100; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10100); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/barometer/invensense/icp10111/CMakeLists.txt b/src/drivers/barometer/invensense/icp10111/CMakeLists.txt new file mode 100755 index 000000000000..ec2a3858b9bc --- /dev/null +++ b/src/drivers/barometer/invensense/icp10111/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__invensense__icp10111 + MAIN icp10111 + COMPILE_FLAGS + SRCS + Inven_Sense_ICP10111_registers.hpp + ICP10111.cpp + ICP10111.hpp + icp10111_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/invensense/icp10111/ICP10111.cpp b/src/drivers/barometer/invensense/icp10111/ICP10111.cpp new file mode 100755 index 000000000000..0b55816749ed --- /dev/null +++ b/src/drivers/barometer/invensense/icp10111/ICP10111.cpp @@ -0,0 +1,378 @@ +/**************************************************************************** + * + * 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 "ICP10111.hpp" + +using namespace time_literals; + +ICP10111::ICP10111(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ICP10111::~ICP10111() +{ + perf_free(_reset_perf); + perf_free(_sample_perf); + perf_free(_bad_transfer_perf); +} + +int +ICP10111::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool +ICP10111::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void +ICP10111::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_reset_perf); + perf_print_counter(_sample_perf); + perf_print_counter(_bad_transfer_perf); +} + +int +ICP10111::probe() +{ + uint16_t ID = 0; + read_response(Cmd::READ_ID, (uint8_t *)&ID, 2); + uint8_t PROD_ID = (ID >> 8) & 0x3f; + + if (PROD_ID != Product_ID) { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void +ICP10111::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // Software Reset + send_command(Cmd::SOFT_RESET); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(100_ms); // Power On Reset: max 100ms + break; + + case STATE::WAIT_FOR_RESET: { + // check product id + uint16_t ID = 0; + read_response(Cmd::READ_ID, (uint8_t *)&ID, 2); + uint8_t PROD_ID = (ID >> 8) & 0x3f; // Product ID Bits 5:0 + + if (PROD_ID == Product_ID) { + // if reset succeeded then read otp + _state = STATE::READ_OTP; + ScheduleDelayed(10_ms); // Time to coefficients are available. + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::READ_OTP: { + // read otp + uint8_t addr_otp_cmd[3] = {0x00, 0x66, 0x9c}; + uint8_t otp_buf[3]; + uint8_t crc; + bool success = true; + + send_command(Cmd::SET_ADDR, addr_otp_cmd, 3); + + for (uint8_t i = 0; i < 4; i++) { + read_response(Cmd::READ_OTP, otp_buf, 3); + + crc = 0xFF; + + for (int j = 0; j < 2; j++) { + crc = (uint8_t)cal_crc(crc, otp_buf[j]); + } + + if (crc != otp_buf[2]) { + success = false; + break; + } + + _scal[i] = (otp_buf[0] << 8) | otp_buf[1]; + } + + if (success) { + _state = STATE::MEASURE; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::MEASURE: + if (Measure()) { + // if configure succeeded then start measurement cycle + _state = STATE::READ; + perf_begin(_sample_perf); + ScheduleDelayed(_measure_interval); + + } else { + // MEASURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Measure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Measure failed, retrying"); + } + + ScheduleDelayed(_measure_interval); + } + + break; + + case STATE::READ: { + uint8_t comp_data[9] {}; + bool success = false; + + if (read_measure_results(comp_data, 9) == PX4_OK) { + perf_end(_sample_perf); + + uint16_t _raw_t = (comp_data[0] << 8) | comp_data[1]; + uint32_t L_res_buf3 = comp_data[3]; // expand result bytes to 32bit to fix issues on 8-bit MCUs + uint32_t L_res_buf4 = comp_data[4]; + uint32_t L_res_buf6 = comp_data[6]; + uint32_t _raw_p = (L_res_buf3 << 16) | (L_res_buf4 << 8) | L_res_buf6; + + // constants for presure calculation + static constexpr float _pcal[3] = { 45000.0, 80000.0, 105000.0 }; + static constexpr float _lut_lower = 3.5 * 0x100000; // 1<<20 + static constexpr float _lut_upper = 11.5 * 0x100000; // 1<<20 + static constexpr float _quadr_factor = 1 / 16777216.0; + static constexpr float _offst_factor = 2048.0; + + // calculate temperature + float _temperature_C = -45.f + 175.f / 65536.f * _raw_t; + + // calculate pressure + float t = (float)(_raw_t - 32768); + float s1 = _lut_lower + (float)(_scal[0] * t * t) * _quadr_factor; + float s2 = _offst_factor * _scal[3] + (float)(_scal[1] * t * t) * _quadr_factor; + float s3 = _lut_upper + (float)(_scal[2] * t * t) * _quadr_factor; + float c = (s1 * s2 * (_pcal[0] - _pcal[1]) + + s2 * s3 * (_pcal[1] - _pcal[2]) + + s3 * s1 * (_pcal[2] - _pcal[0])) / + (s3 * (_pcal[0] - _pcal[1]) + + s1 * (_pcal[1] - _pcal[2]) + + s2 * (_pcal[2] - _pcal[0])); + float a = (_pcal[0] * s1 - _pcal[1] * s2 - (_pcal[1] - _pcal[0]) * c) / (s1 - s2); + float b = (_pcal[0] - a) * (s1 + c); + float _pressure_Pa = a + b / (c + _raw_p); + + float temperature = _temperature_C; + float pressure = _pressure_Pa; // to Pascal + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = now; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = pressure; + sensor_baro.temperature = temperature; + sensor_baro.error_count = perf_event_count(_bad_transfer_perf); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + + _state = STATE::MEASURE; + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + ScheduleDelayed(1000_ms / 8 - _measure_interval); // 8Hz + } + + break; + } +} + +bool +ICP10111::Measure() +{ + /* + From ds-000186-icp-101xx-v1.0.pdf, page 6, table 1 + + Sensor Measurement Max Time + Mode Time (Forced) + Low Power (LP) 1.6 ms 1.8 ms + Normal (N) 5.6 ms 6.3 ms + Low Noise (LN) 20.8 ms 23.8 ms + Ultra Low Noise(ULN) 83.2 ms 94.5 ms + */ + Cmd cmd; + + switch (_mode) { + case MODE::FAST: + cmd = Cmd::MEAS_LP; + _measure_interval = 2_ms; + break; + + case MODE::ACCURATE: + cmd = Cmd::MEAS_LN; + _measure_interval = 24_ms; + break; + + case MODE::VERY_ACCURATE: + cmd = Cmd::MEAS_ULN; + _measure_interval = 95_ms; + break; + + case MODE::NORMAL: + default: + cmd = Cmd::MEAS_N; + _measure_interval = 7_ms; + break; + } + + if (send_command(cmd) != PX4_OK) { + return false; + } + + return true; +} + +int8_t +ICP10111::cal_crc(uint8_t seed, uint8_t data) +{ + int8_t poly = 0x31; + int8_t var2; + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((seed & 0x80) ^ (data & 0x80)) { + var2 = 1; + + } else { + var2 = 0; + } + + seed = (seed & 0x7F) << 1; + data = (data & 0x7F) << 1; + seed = seed ^ (uint8_t)(poly * var2); + } + + return (int8_t)seed; +} + +int +ICP10111::read_measure_results(uint8_t *buf, uint8_t len) +{ + return transfer(nullptr, 0, buf, len); +} + +int +ICP10111::read_response(Cmd cmd, uint8_t *buf, uint8_t len) +{ + uint8_t buff[2]; + buff[0] = ((uint16_t)cmd >> 8) & 0xff; + buff[1] = (uint16_t)cmd & 0xff; + return transfer(&buff[0], 2, buf, len); +} + +int +ICP10111::send_command(Cmd cmd) +{ + uint8_t buf[2]; + buf[0] = ((uint16_t)cmd >> 8) & 0xff; + buf[1] = (uint16_t)cmd & 0xff; + return transfer(buf, sizeof(buf), nullptr, 0); +} + +int +ICP10111::send_command(Cmd cmd, uint8_t *data, uint8_t len) +{ + uint8_t buf[5]; + buf[0] = ((uint16_t)cmd >> 8) & 0xff; + buf[1] = (uint16_t)cmd & 0xff; + memcpy(&buf[2], data, len); + return transfer(&buf[0], len + 2, nullptr, 0); +} diff --git a/src/drivers/barometer/invensense/icp10111/ICP10111.hpp b/src/drivers/barometer/invensense/icp10111/ICP10111.hpp new file mode 100755 index 000000000000..54ace11369b4 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10111/ICP10111.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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 + +#include "Inven_Sense_ICP10111_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace Inven_Sense_ICP10111; + +class ICP10111 : public device::I2C, public I2CSPIDriver +{ +public: + ICP10111(const I2CSPIDriverConfig &config); + ~ICP10111() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + int probe() override; + + bool Reset(); + + bool Measure(); + + int8_t cal_crc(uint8_t seed, uint8_t data); + int read_measure_results(uint8_t *buf, uint8_t len); + int read_response(Cmd cmd, uint8_t *buf, uint8_t len); + int send_command(Cmd cmd); + int send_command(Cmd cmd, uint8_t *data, uint8_t len); + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + + hrt_abstime _reset_timestamp{0}; + int _failure_count{0}; + + unsigned _measure_interval{0}; + int16_t _scal[4]; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + READ_OTP, + MEASURE, + READ + } _state{STATE::RESET}; + + enum class MODE : uint8_t { + FAST, + NORMAL, + ACCURATE, + VERY_ACCURATE + } _mode{MODE::VERY_ACCURATE}; +}; diff --git a/src/drivers/barometer/invensense/icp10111/Inven_Sense_ICP10111_registers.hpp b/src/drivers/barometer/invensense/icp10111/Inven_Sense_ICP10111_registers.hpp new file mode 100755 index 000000000000..907fa1b3241e --- /dev/null +++ b/src/drivers/barometer/invensense/icp10111/Inven_Sense_ICP10111_registers.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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 icp10111_registers.hpp + * + * icp10111 registers. + * + */ + +#pragma once + +#include + +namespace Inven_Sense_ICP10111 +{ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x63; + +static constexpr uint8_t Product_ID = 0x08; + +enum class Cmd : uint16_t { + READ_ID = 0xefc8, + SET_ADDR = 0xc595, + READ_OTP = 0xc7f7, + MEAS_LP = 0x609c, + MEAS_N = 0x6825, + MEAS_LN = 0x70df, + MEAS_ULN = 0x7866, + SOFT_RESET = 0x805d +}; +} // namespace Inven_Sense_ICP10111 diff --git a/src/drivers/barometer/invensense/icp10111/icp10111_main.cpp b/src/drivers/barometer/invensense/icp10111/icp10111_main.cpp new file mode 100755 index 000000000000..c7cfb3d2cec6 --- /dev/null +++ b/src/drivers/barometer/invensense/icp10111/icp10111_main.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 + +#include "ICP10111.hpp" + +void +ICP10111::print_usage() +{ + PRINT_MODULE_USAGE_NAME("icp10111", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x63); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int icp10111_main(int argc, char *argv[]) +{ + using ThisDriver = ICP10111; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_ICP10111); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/barometer/lps22hb/CMakeLists.txt b/src/drivers/barometer/lps22hb/CMakeLists.txt index 41652d1c5199..3014c8c28468 100644 --- a/src/drivers/barometer/lps22hb/CMakeLists.txt +++ b/src/drivers/barometer/lps22hb/CMakeLists.txt @@ -39,6 +39,5 @@ px4_add_module( LPS22HB_I2C.cpp LPS22HB_SPI.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps22hb/Kconfig b/src/drivers/barometer/lps22hb/Kconfig new file mode 100644 index 000000000000..6823384732f1 --- /dev/null +++ b/src/drivers/barometer/lps22hb/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_LPS22HB + bool "lps22hb" + default n + ---help--- + Enable support for lps22hb \ No newline at end of file diff --git a/src/drivers/barometer/lps22hb/LPS22HB.cpp b/src/drivers/barometer/lps22hb/LPS22HB.cpp index 1c09faac15d9..c3af636dd959 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.cpp @@ -42,9 +42,8 @@ /* Max measurement rate is 25Hz */ #define LPS22HB_CONVERSION_INTERVAL (1000000 / 25) /* microseconds */ -LPS22HB::LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_baro(interface->get_device_id()), +LPS22HB::LPS22HB(const I2CSPIDriverConfig &config, device::Device *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) @@ -152,14 +151,23 @@ int LPS22HB::collect() uint32_t TEMP_OUT = report.TEMP_OUT_L + (report.TEMP_OUT_H << 8); float temperature = 42.5f + (TEMP_OUT / 480.0f); - _px4_baro.set_temperature(temperature); // To obtain the pressure in hPa, take the two’s complement of the complete word and then divide by 4096 LSB/hPa. uint32_t P = report.PRESS_OUT_XL + (report.PRESS_OUT_L << 8) + (report.PRESS_OUT_H << 16); /* Pressure and MSL in mBar */ float pressure = P / 4096.0f; - _px4_baro.update(timestamp_sample, pressure); + float pressure_pa = pressure * 100.f; + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = pressure_pa; + 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 PX4_OK; diff --git a/src/drivers/barometer/lps22hb/LPS22HB.hpp b/src/drivers/barometer/lps22hb/LPS22HB.hpp index b95c254149e8..b0f2141690df 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.hpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.hpp @@ -39,7 +39,8 @@ #include #include #include -#include +#include +#include static constexpr uint8_t WHO_AM_I = 0x0F; static constexpr uint8_t LPS22HB_ID_WHO_AM_I = 0xB1; @@ -70,6 +71,8 @@ static constexpr uint8_t PRESS_OUT_H = 0x2A; static constexpr uint8_t TEMP_OUT_L = 0x2B; static constexpr uint8_t TEMP_OUT_H = 0x2C; +#define LPS22HB_ADDRESS 0x5D + /* interface factories */ extern device::Device *LPS22HB_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); extern device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency); @@ -77,11 +80,10 @@ extern device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency); class LPS22HB : public I2CSPIDriver { public: - LPS22HB(I2CSPIBusOption bus_option, int bus, device::Device *interface); + LPS22HB(const I2CSPIDriverConfig &config, device::Device *interface); virtual ~LPS22HB(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -91,7 +93,8 @@ class LPS22HB : public I2CSPIDriver void RunImpl(); private: - PX4Barometer _px4_baro; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + device::Device *_interface; bool _collect_phase{false}; diff --git a/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp index 1b4a3dee5910..8d4f65751284 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp +++ b/src/drivers/barometer/lps22hb/LPS22HB_I2C.cpp @@ -41,8 +41,6 @@ #include -#define LPS22HB_ADDRESS 0x5D - device::Device *LPS22HB_I2C_interface(int bus, int bus_frequency); class LPS22HB_I2C : public device::I2C @@ -74,20 +72,18 @@ int LPS22HB_I2C::probe() { uint8_t id = 0; - _retries = 10; - if (read(WHO_AM_I, &id, 1)) { DEVICE_DEBUG("read_reg fail"); return -EIO; } - _retries = 2; - if (id != LPS22HB_ID_WHO_AM_I) { DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", LPS22HB_ID_WHO_AM_I, id); return -EIO; } + _retries = 1; + return PX4_OK; } diff --git a/src/drivers/barometer/lps22hb/lps22hb_main.cpp b/src/drivers/barometer/lps22hb/lps22hb_main.cpp index 1c4bf5bb4efe..a39198938b0a 100644 --- a/src/drivers/barometer/lps22hb/lps22hb_main.cpp +++ b/src/drivers/barometer/lps22hb/lps22hb_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -43,30 +43,29 @@ void LPS22HB::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LPS22HB::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *LPS22HB::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = LPS22HB_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = LPS22HB_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = LPS22HB_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = LPS22HB_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } - LPS22HB *dev = new LPS22HB(iterator.configuredBusOption(), iterator.bus(), interface); + LPS22HB *dev = new LPS22HB(config, interface); if (dev == nullptr) { delete interface; @@ -95,6 +94,8 @@ extern "C" __EXPORT int lps22hb_main(int argc, char *argv[]) return -1; } + cli.i2c_address = LPS22HB_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_LPS22HB); if (!strcmp(verb, "start")) { diff --git a/src/drivers/barometer/lps25h/CMakeLists.txt b/src/drivers/barometer/lps25h/CMakeLists.txt index 778221bd41cf..4cc2841f0e70 100644 --- a/src/drivers/barometer/lps25h/CMakeLists.txt +++ b/src/drivers/barometer/lps25h/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( lps25h_i2c.cpp lps25h_spi.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps25h/Kconfig b/src/drivers/barometer/lps25h/Kconfig new file mode 100644 index 000000000000..fa17e99ed9c7 --- /dev/null +++ b/src/drivers/barometer/lps25h/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_LPS25H + bool "lps25h" + default n + ---help--- + Enable support for lps25h \ No newline at end of file diff --git a/src/drivers/barometer/lps25h/LPS25H.cpp b/src/drivers/barometer/lps25h/LPS25H.cpp index c08aeaa39268..ed28d0b456d5 100644 --- a/src/drivers/barometer/lps25h/LPS25H.cpp +++ b/src/drivers/barometer/lps25h/LPS25H.cpp @@ -33,9 +33,8 @@ #include "LPS25H.hpp" -LPS25H::LPS25H(I2CSPIBusOption bus_option, int bus, device::Device *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_barometer(interface->get_device_id()), +LPS25H::LPS25H(const I2CSPIDriverConfig &config, device::Device *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")) @@ -162,19 +161,25 @@ int LPS25H::collect() return ret; } - _px4_barometer.set_error_count(perf_event_count(_comms_errors)); - /* get measurements from the device */ float temperature = 42.5f + (report.t / 480); - _px4_barometer.set_temperature(temperature); /* raw pressure */ uint32_t raw = report.p_xl + (report.p_l << 8) + (report.p_h << 16); /* Pressure and MSL in mBar */ float pressure = raw / 4096.0f; - - _px4_barometer.update(timestamp_sample, pressure); + float pressure_pa = pressure * 100.f; + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = pressure_pa; + 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 PX4_OK; diff --git a/src/drivers/barometer/lps25h/LPS25H.hpp b/src/drivers/barometer/lps25h/LPS25H.hpp index e1b0996dfc3a..cdd41ab5a14e 100644 --- a/src/drivers/barometer/lps25h/LPS25H.hpp +++ b/src/drivers/barometer/lps25h/LPS25H.hpp @@ -43,7 +43,8 @@ #include #include -#include +#include +#include #include #include @@ -155,11 +156,10 @@ class LPS25H : public I2CSPIDriver { public: - LPS25H(I2CSPIBusOption bus_option, int bus, device::Device *interface); + LPS25H(const I2CSPIDriverConfig &config, device::Device *interface); ~LPS25H() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -177,7 +177,8 @@ class LPS25H : public I2CSPIDriver int measure(); int collect(); - PX4Barometer _px4_barometer; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + device::Device *_interface; unsigned _measure_interval{0}; diff --git a/src/drivers/barometer/lps25h/lps25h.h b/src/drivers/barometer/lps25h/lps25h.h index a16a1c25570a..57b18944cc70 100644 --- a/src/drivers/barometer/lps25h/lps25h.h +++ b/src/drivers/barometer/lps25h/lps25h.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016, 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 @@ -50,6 +50,8 @@ #define ID_WHO_AM_I 0xBD +#define LPS25H_ADDRESS 0x5D + /* interface factories */ extern device::Device *LPS25H_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); extern device::Device *LPS25H_I2C_interface(int bus, int bus_frequency); diff --git a/src/drivers/barometer/lps25h/lps25h_i2c.cpp b/src/drivers/barometer/lps25h/lps25h_i2c.cpp index 03ecf2c9e50f..46a2cc8a17e3 100644 --- a/src/drivers/barometer/lps25h/lps25h_i2c.cpp +++ b/src/drivers/barometer/lps25h/lps25h_i2c.cpp @@ -41,8 +41,6 @@ #include -#define LPS25H_ADDRESS 0x5D - device::Device *LPS25H_I2C_interface(int bus, int bus_frequency); class LPS25H_I2C : public device::I2C @@ -73,20 +71,18 @@ int LPS25H_I2C::probe() { uint8_t id; - _retries = 10; - if (read(ADDR_WHO_AM_I, &id, 1)) { DEVICE_DEBUG("read_reg fail"); return -EIO; } - _retries = 2; - if (id != ID_WHO_AM_I) { DEVICE_DEBUG("ID byte mismatch (%02x != %02x)", ID_WHO_AM_I, id); return -EIO; } + _retries = 1; + return OK; } diff --git a/src/drivers/barometer/lps25h/lps25h_main.cpp b/src/drivers/barometer/lps25h/lps25h_main.cpp index 0b2f7c6ff350..564a53905c04 100644 --- a/src/drivers/barometer/lps25h/lps25h_main.cpp +++ b/src/drivers/barometer/lps25h/lps25h_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -47,30 +47,29 @@ LPS25H::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LPS25H::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *LPS25H::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = LPS25H_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = LPS25H_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = LPS25H_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = LPS25H_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } - LPS25H *dev = new LPS25H(iterator.configuredBusOption(), iterator.bus(), interface); + LPS25H *dev = new LPS25H(config, interface); if (dev == nullptr) { delete interface; @@ -99,6 +98,8 @@ extern "C" int lps25h_main(int argc, char *argv[]) return -1; } + cli.i2c_address = LPS25H_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_LPS25H); if (!strcmp(verb, "start")) { diff --git a/src/drivers/barometer/lps33hw/CMakeLists.txt b/src/drivers/barometer/lps33hw/CMakeLists.txt index 6e8ce1c9eab6..800262a1ab7b 100644 --- a/src/drivers/barometer/lps33hw/CMakeLists.txt +++ b/src/drivers/barometer/lps33hw/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( lps33hw_spi.cpp lps33hw_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/lps33hw/Kconfig b/src/drivers/barometer/lps33hw/Kconfig new file mode 100644 index 000000000000..bcf7f33edd6a --- /dev/null +++ b/src/drivers/barometer/lps33hw/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_LPS33HW + bool "lps33hw" + default n + ---help--- + Enable support for lps33hw \ No newline at end of file diff --git a/src/drivers/barometer/lps33hw/lps33hw.cpp b/src/drivers/barometer/lps33hw/lps33hw.cpp index ee36e6996fbb..00d821791ca1 100644 --- a/src/drivers/barometer/lps33hw/lps33hw.cpp +++ b/src/drivers/barometer/lps33hw/lps33hw.cpp @@ -47,14 +47,12 @@ static void getTwosComplement(T &raw, uint8_t length) } } -LPS33HW::LPS33HW(I2CSPIBusOption bus_option, int bus, device::Device *interface, bool keep_retrying) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, - interface->get_device_address()), - _px4_barometer(interface->get_device_id()), +LPS33HW::LPS33HW(const I2CSPIDriverConfig &config, device::Device *interface) : + I2CSPIDriver(config), _interface(interface), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comm errors")), - _keep_retrying(keep_retrying) + _keep_retrying(config.keep_running) { } @@ -178,10 +176,17 @@ LPS33HW::RunImpl() int32_t Praw = (int32_t)data[1] | (data[2] << 8) | (data[3] << 16); getTwosComplement(Praw, 24); float pressure_hPa = Praw / 4096.f; - - _px4_barometer.set_error_count(perf_event_count(_comms_errors)); - _px4_barometer.set_temperature(temp); - _px4_barometer.update(timestamp_sample, pressure_hPa); // hPascals -> Millibar + float pressure_pa = pressure_hPa * 100.f; + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = pressure_pa; + sensor_baro.temperature = temp; + 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); ScheduleDelayed(1000000 / SAMPLE_RATE); diff --git a/src/drivers/barometer/lps33hw/lps33hw.hpp b/src/drivers/barometer/lps33hw/lps33hw.hpp index 5539e666bc48..b779ce4fdbea 100644 --- a/src/drivers/barometer/lps33hw/lps33hw.hpp +++ b/src/drivers/barometer/lps33hw/lps33hw.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -40,7 +40,8 @@ #pragma once #include -#include +#include +#include #include #include #include @@ -55,11 +56,10 @@ using ST_LPS33HW::Register; class LPS33HW : public I2CSPIDriver { public: - LPS33HW(I2CSPIBusOption bus_option, int bus, device::Device *interface, bool keep_retrying); + LPS33HW(const I2CSPIDriverConfig &config, device::Device *interface); virtual ~LPS33HW(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -83,7 +83,7 @@ class LPS33HW : public I2CSPIDriver static constexpr uint32_t SAMPLE_RATE{75}; - PX4Barometer _px4_barometer; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; device::Device *_interface; diff --git a/src/drivers/barometer/lps33hw/lps33hw_main.cpp b/src/drivers/barometer/lps33hw/lps33hw_main.cpp index 168b0562f358..47cfdd829915 100644 --- a/src/drivers/barometer/lps33hw/lps33hw_main.cpp +++ b/src/drivers/barometer/lps33hw/lps33hw_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -56,30 +56,29 @@ LPS33HW::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LPS33HW::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *LPS33HW::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = LPS33HW_I2C_interface(iterator.bus(), cli.i2c_address, cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = LPS33HW_I2C_interface(config.bus, config.i2c_address, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = LPS33HW_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = LPS33HW_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); return nullptr; } - LPS33HW *dev = new LPS33HW(iterator.configuredBusOption(), iterator.bus(), interface, cli.keep_running); + LPS33HW *dev = new LPS33HW(config, interface); if (dev == nullptr) { delete interface; diff --git a/src/drivers/barometer/maiertek/CMakeLists.txt b/src/drivers/barometer/maiertek/CMakeLists.txt new file mode 100644 index 000000000000..73cc58cde7df --- /dev/null +++ b/src/drivers/barometer/maiertek/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(mpc2520) diff --git a/src/drivers/barometer/maiertek/Kconfig b/src/drivers/barometer/maiertek/Kconfig new file mode 100644 index 000000000000..ace1229d8a00 --- /dev/null +++ b/src/drivers/barometer/maiertek/Kconfig @@ -0,0 +1,3 @@ +#menu "Maiertek" +rsource "*/Kconfig" +#endmenu #Maiertek diff --git a/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt b/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt new file mode 100644 index 000000000000..c778044cb58b --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2020-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. +# +############################################################################ + +px4_add_module( + MODULE drivers__maiertek__mpc2520 + MAIN mpc2520 + COMPILE_FLAGS + SRCS + MaierTek_MPC2520_registers.hpp + mpc2520_main.cpp + MPC2520.cpp + MPC2520.hpp + DEPENDS + px4_work_queue +) diff --git a/src/drivers/barometer/maiertek/mpc2520/Kconfig b/src/drivers/barometer/maiertek/mpc2520/Kconfig new file mode 100644 index 000000000000..fdf7ebbb9db0 --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_MAIERTEK_MPC2520 + bool "MPC2520" + default n + ---help--- + Enable support for MPC2520 diff --git a/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp b/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp new file mode 100644 index 000000000000..165c7853c40b --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/MPC2520.cpp @@ -0,0 +1,354 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "MPC2520.hpp" + +using namespace time_literals; + +static constexpr int32_t combine(uint8_t h, uint8_t m, uint8_t l) +{ + // 24 bit sign extend + int32_t ret = (uint32_t)(h << 24) | (uint32_t)(m << 16) | (uint32_t)(l << 8); + return ret >> 8; +} + +MPC2520::MPC2520(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ + //_debug_enabled = true; +} + +MPC2520::~MPC2520() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int MPC2520::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool MPC2520::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void MPC2520::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int MPC2520::probe() +{ + const uint8_t ID = RegisterRead(Register::ID); + + uint8_t PROD_ID = ID & 0xF0 >> 4; // Product ID Bits 7:4 + + if (PROD_ID != Product_ID) { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void MPC2520::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // RESET: SOFT_RST + RegisterWrite(Register::RESET, RESET_BIT::SOFT_RST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(50_ms); // Power On Reset: max 50ms + break; + + case STATE::WAIT_FOR_RESET: { + // check MEAS_CFG SENSOR_RDY + const uint8_t ID = RegisterRead(Register::ID); + + uint8_t PROD_ID = ID & 0xF0 >> 4; // Product ID Bits 7:4 + + if (PROD_ID == Product_ID) { + // if reset succeeded then read prom + _state = STATE::READ_PROM; + ScheduleDelayed(40_ms); // Time to coefficients are available. + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + + break; + + case STATE::READ_PROM: { + uint8_t prom_buf[3] {}; + uint8_t cmd = 0x10; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c0 = (int16_t)prom_buf[0] << 4 | prom_buf[1] >> 4; + _prom.c0 = (_prom.c0 & 0x0800) ? (0xF000 | _prom.c0) : _prom.c0; + + cmd = 0x11; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c1 = (int16_t)(prom_buf[0] & 0x0F) << 8 | prom_buf[1]; + _prom.c1 = (_prom.c1 & 0x0800) ? (0xF000 | _prom.c1) : _prom.c1; + + cmd = 0x13; + transfer(&cmd, 1, &prom_buf[0], 3); + _prom.c00 = (int32_t)prom_buf[0] << 12 | (int32_t)prom_buf[1] << 4 | (int32_t)prom_buf[2] >> 4; + _prom.c00 = (_prom.c00 & 0x080000) ? (0xFFF00000 | _prom.c00) : _prom.c00; + + cmd = 0x15; + transfer(&cmd, 1, &prom_buf[0], 3); + _prom.c10 = (int32_t)prom_buf[0] << 16 | (int32_t)prom_buf[1] << 8 | prom_buf[2]; + _prom.c10 = (_prom.c10 & 0x080000) ? (0xFFF00000 | _prom.c10) : _prom.c10; + + cmd = 0x18; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c01 = (int16_t)prom_buf[0] << 8 | prom_buf[1]; + + cmd = 0x1A; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c11 = (int16_t)prom_buf[0] << 8 | prom_buf[1]; + + cmd = 0x1C; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c20 = (int16_t)prom_buf[0] << 8 | prom_buf[1]; + + cmd = 0x1E; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c21 = (int16_t)prom_buf[0] << 8 | prom_buf[1]; + + cmd = 0x20; + transfer(&cmd, 1, &prom_buf[0], 2); + _prom.c30 = (int16_t)prom_buf[0] << 8 | prom_buf[1]; + + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + } + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start measurement cycle + _state = STATE::READ; + ScheduleOnInterval(1000000 / 32); // 32 Hz + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + bool success = false; + + // read temperature first + struct TransferBuffer { + uint8_t PSR_B2; + uint8_t PSR_B1; + uint8_t PSR_B0; + uint8_t TMP_B2; + uint8_t TMP_B1; + uint8_t TMP_B0; + } buffer{}; + + uint8_t cmd = static_cast(Register::PSR_B2); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + int32_t Traw = (int32_t)(buffer.TMP_B2 << 16) | (int32_t)(buffer.TMP_B1 << 8) | (int32_t)buffer.TMP_B0; + Traw = (Traw & 0x800000) ? (0xFF000000 | Traw) : Traw; + + static constexpr float kT = 7864320; // temperature 8 times oversampling + float Traw_sc = static_cast(Traw) / kT; + float Tcomp = _prom.c0 * 0.5f + _prom.c1 * Traw_sc; + + int32_t Praw = (int32_t)(buffer.PSR_B2 << 16) | (int32_t)(buffer.PSR_B1 << 8) | (int32_t)buffer.PSR_B1; + Praw = (Praw & 0x800000) ? (0xFF000000 | Praw) : Praw; + + static constexpr float kP = 7864320; // pressure 8 times oversampling + float Praw_sc = static_cast(Praw) / kP; + + // Calculate compensated measurement results. + float Pcomp = _prom.c00 + Praw_sc * (_prom.c10 + Praw_sc * (_prom.c20 + Praw_sc * _prom.c30)) + Traw_sc * _prom.c01 + + Traw_sc * Praw_sc * (_prom.c11 + Praw_sc * _prom.c21); + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = now; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = Pcomp; + sensor_baro.temperature = Tcomp; + sensor_baro.error_count = perf_event_count(_bad_transfer_perf) + perf_event_count(_bad_register_perf); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + 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])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + return; + } + } + } + + break; + } +} + +bool MPC2520::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + return success; +} + +bool MPC2520::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t MPC2520::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void MPC2520::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void MPC2520::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp b/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp new file mode 100644 index 000000000000..7cc110dbb966 --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/MPC2520.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 + +#include "MaierTek_MPC2520_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace MaierTek_MPC2520; + +class MPC2520 : public device::I2C, public I2CSPIDriver +{ +public: + MPC2520(const I2CSPIDriverConfig &config); + ~MPC2520() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + +#pragma pack(push,1) + struct prom_s { + int16_t c0; + int16_t c1; + int32_t c00; + int32_t c10; + int16_t c01; + int16_t c11; + int16_t c20; + int16_t c21; + int16_t c30; + } _prom{}; +#pragma pack(pop) + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + READ_PROM, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{4}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::PRS_CFG, PRS_CFG_BIT::PM_RATE_32_SET | PRS_CFG_BIT::PM_PRC_8_SET, PRS_CFG_BIT::PM_RATE_32_CLEAR | PRS_CFG_BIT::PM_PRC_8_CLEAR }, + { Register::TMP_CFG, TMP_CFG_BIT::TMP_EXT | TMP_CFG_BIT::TMP_RATE_32_SET | TMP_CFG_BIT::TMP_PRC_8_SET, TMP_CFG_BIT::TMP_RATE_32_CLEAR | TMP_CFG_BIT::TMP_PRC_8_CLEAR }, + { Register::MEAS_CFG, MEAS_CFG_BIT::MEAS_CTRL_CONT_PRES_TEMP, 0 }, + { Register::CFG_REG, 0, CFG_REG_BIT::T_SHIFT | CFG_REG_BIT::P_SHIFT }, + }; +}; diff --git a/src/drivers/barometer/maiertek/mpc2520/MaierTek_MPC2520_registers.hpp b/src/drivers/barometer/maiertek/mpc2520/MaierTek_MPC2520_registers.hpp new file mode 100644 index 000000000000..75ec48ff8ee8 --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/MaierTek_MPC2520_registers.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 MaierTek_MPC2520_registers.hpp + * + * MaierTek MPC2520 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace MaierTek_MPC2520 +{ +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x76; + +static constexpr uint8_t Product_ID = 0x0; +static constexpr uint8_t Revision_ID = 0x0; + +enum class Register : uint8_t { + PSR_B2 = 0x00, // PSR[23:16] (r) + PSR_B1 = 0x01, // PSR[15:8] (r) + PSR_B0 = 0x02, // PSR[7:0] (r) + TMP_B2 = 0x03, // TMP[23:16] (r) + TMP_B1 = 0x04, // TMP[15:8] (r) + TMP_B0 = 0x05, // TMP[7:0] (r) + PRS_CFG = 0x06, + TMP_CFG = 0x07, + MEAS_CFG = 0x08, + CFG_REG = 0x09, + + RESET = 0x0C, + ID = 0x0D, // PROD_ID [3:0] (r) REV_ID [3:0] (r) + + COEF = 0x10, +}; + +// PRS_CFG +enum PRS_CFG_BIT : uint8_t { + // PM_RATE[6:4] + PM_RATE_32_SET = Bit6 | Bit4, // 101 - 32 measurements pr. sec. + PM_RATE_32_CLEAR = Bit5, + + // PM_PRC[3:0] + PM_PRC_8_SET = Bit1 | Bit0, // 0011 - 8 times. + PM_PRC_8_CLEAR = Bit3 | Bit2, +}; + +// TMP_CFG +enum TMP_CFG_BIT : uint8_t { + TMP_EXT = Bit7, + + // TMP_RATE[6:4] + TMP_RATE_32_SET = Bit6 | Bit4, // 101 - 32 measurements pr. sec. + TMP_RATE_32_CLEAR = Bit5, + + // PM_PRC[2:0] + TMP_PRC_8_SET = Bit1 | Bit0, // 011 - 8 times. + TMP_PRC_8_CLEAR = Bit2, +}; + +// MEAS_CFG +enum MEAS_CFG_BIT : uint8_t { + COEF_RDY = Bit7, + SENSOR_RDY = Bit6, + TMP_RDY = Bit5, + PRS_RDY = Bit4, + + MEAS_CTRL_CONT_PRES_TEMP = Bit2 | Bit1 | Bit0, // 111 - Continuous pressure and temperature measurement +}; + +// RESET +enum RESET_BIT : uint8_t { + SOFT_RST = Bit3 | Bit0, // Write '1001' to generate a soft reset. +}; + +// CFG_REG +enum CFG_REG_BIT : uint8_t { + T_SHIFT = Bit3, // Temperature result bit-shift, Note: Must be set to '1' when the oversampling rate is >8 times. + P_SHIFT = Bit2, // Pressure result bit-shift, Note: Must be set to '1' when the oversampling rate is >8 times. +}; + +} // namespace MaierTek_MPC2520 diff --git a/src/drivers/barometer/maiertek/mpc2520/mpc2520_main.cpp b/src/drivers/barometer/maiertek/mpc2520/mpc2520_main.cpp new file mode 100644 index 000000000000..6df2fb139813 --- /dev/null +++ b/src/drivers/barometer/maiertek/mpc2520/mpc2520_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "MPC2520.hpp" + +#include +#include + +void MPC2520::print_usage() +{ + PRINT_MODULE_USAGE_NAME("mpc2520", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int mpc2520_main(int argc, char *argv[]) +{ + using ThisDriver = MPC2520; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_MPC2520); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/barometer/mpl3115a2/CMakeLists.txt b/src/drivers/barometer/mpl3115a2/CMakeLists.txt index c96c24dfd6cd..6ad33fcdce46 100644 --- a/src/drivers/barometer/mpl3115a2/CMakeLists.txt +++ b/src/drivers/barometer/mpl3115a2/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( MPL3115A2.cpp mpl3115a2_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/mpl3115a2/Kconfig b/src/drivers/barometer/mpl3115a2/Kconfig new file mode 100644 index 000000000000..6d95e95af6a4 --- /dev/null +++ b/src/drivers/barometer/mpl3115a2/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_MPL3115A2 + bool "mpl3115a2" + default n + ---help--- + Enable support for mpl3115a2 \ No newline at end of file diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp index 7c3ca88b9090..8acc20246543 100644 --- a/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.cpp @@ -33,8 +33,6 @@ #include "MPL3115A2.hpp" -#define MPL3115A2_ADDRESS 0x60 - #define MPL3115A2_REG_WHO_AM_I 0x0c #define MPL3115A2_WHO_AM_I 0xC4 @@ -54,10 +52,9 @@ #define MPL3115A2_OSR 2 /* Over Sample rate of 4 18MS Minimum time between data samples */ #define MPL3115A2_CTRL_TRIGGER (CTRL_REG1_OST | CTRL_REG1_OS(MPL3115A2_OSR)) -MPL3115A2::MPL3115A2(I2CSPIBusOption bus_option, const int bus, int bus_frequency) : - I2C(DRV_BARO_DEVTYPE_MPL3115A2, MODULE_NAME, bus, MPL3115A2_ADDRESS, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_barometer(get_device_id()), +MPL3115A2::MPL3115A2(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) @@ -87,18 +84,15 @@ int MPL3115A2::init() int MPL3115A2::probe() { - _retries = 10; uint8_t whoami = 0; if ((RegisterRead(MPL3115A2_REG_WHO_AM_I, &whoami) > 0) && (whoami == MPL3115A2_WHO_AM_I)) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; + return PX4_OK; } + _retries = 1; + return -EIO; } @@ -200,11 +194,6 @@ int MPL3115A2::measure() // Send the command to read the ADC for P and T. unsigned addr = (MPL3115A2_CTRL_REG1 << 8) | MPL3115A2_CTRL_TRIGGER; - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; int ret = RegisterWrite((addr >> 8) & 0xff, addr & 0xff); if (ret == -EIO) { @@ -270,9 +259,15 @@ int MPL3115A2::collect() float T = (float) reading.temperature.b[1] + ((float)(reading.temperature.b[0]) / 16.0f); float P = (float)(reading.pressure.q >> 8) + ((float)(reading.pressure.b[0]) / 4.0f); - _px4_barometer.set_error_count(perf_event_count(_comms_errors)); - _px4_barometer.set_temperature(T); - _px4_barometer.update(timestamp_sample, P / 100.0f); + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = P; + sensor_baro.temperature = T; + 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); diff --git a/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp index fccf0f274f84..9bf8bf7f51e1 100644 --- a/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp +++ b/src/drivers/barometer/mpl3115a2/MPL3115A2.hpp @@ -42,20 +42,21 @@ #include #include #include -#include +#include +#include #include #include #include #include +#define MPL3115A2_ADDRESS 0x60 + class MPL3115A2 : public device::I2C, public I2CSPIDriver { public: - MPL3115A2(I2CSPIBusOption bus_option, const int bus, int bus_frequency); + MPL3115A2(const I2CSPIDriverConfig &config); ~MPL3115A2() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); @@ -76,7 +77,7 @@ class MPL3115A2 : public device::I2C, public I2CSPIDriver int RegisterRead(uint8_t reg, void *data, unsigned count = 1); int RegisterWrite(uint8_t reg, uint8_t data); - PX4Barometer _px4_barometer; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; bool _collect_phase{false}; diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp b/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp index ea9d61d67a0f..19b6312db3fd 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2_main.cpp @@ -43,31 +43,16 @@ MPL3115A2::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("baro"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x60); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPL3115A2::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MPL3115A2 *dev = new MPL3115A2(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - - if (dev == nullptr) { - return nullptr; - } - - if (OK != dev->init()) { - delete dev; - return nullptr; - } - - return dev; -} - extern "C" int mpl3115a2_main(int argc, char *argv[]) { using ThisDriver = MPL3115A2; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 400000; + cli.i2c_address = MPL3115A2_ADDRESS; const char *verb = cli.parseDefaultArguments(argc, argv); diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index f67fb270d8a5..ce693bf63a21 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MODULE drivers__barometer__ms5611 MAIN ms5611 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS ms5611_spi.cpp ms5611_i2c.cpp @@ -45,6 +44,5 @@ px4_add_module( DEPENDS cdev drivers__device - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/ms5611/Kconfig b/src/drivers/barometer/ms5611/Kconfig new file mode 100644 index 000000000000..849894ba2720 --- /dev/null +++ b/src/drivers/barometer/ms5611/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_MS5611 + bool "ms5611" + default n + ---help--- + Enable support for ms5611 \ No newline at end of file diff --git a/src/drivers/barometer/ms5611/MS5611.hpp b/src/drivers/barometer/ms5611/MS5611.hpp index 3003cb8e50fc..6f297e86b4d8 100644 --- a/src/drivers/barometer/ms5611/MS5611.hpp +++ b/src/drivers/barometer/ms5611/MS5611.hpp @@ -34,7 +34,8 @@ #pragma once #include -#include +#include +#include #include #include #include @@ -87,12 +88,10 @@ enum MS56XX_DEVICE_TYPES { class MS5611 : public I2CSPIDriver { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type, - I2CSPIBusOption bus_option, int bus); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config); ~MS5611() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); @@ -115,7 +114,7 @@ class MS5611 : public I2CSPIDriver protected: void print_status() override; - PX4Barometer _px4_barometer; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; device::Device *_interface; @@ -129,6 +128,11 @@ class MS5611 : public I2CSPIDriver int64_t _OFF{0}; int64_t _SENS{0}; + bool _initialized{false}; + + float _last_pressure{NAN}; + float _last_temperature{NAN}; + perf_counter_t _sample_perf; perf_counter_t _measure_perf; perf_counter_t _comms_errors; diff --git a/src/drivers/barometer/ms5611/ms5611.cpp b/src/drivers/barometer/ms5611/ms5611.cpp index 480237a958d8..06a4a04db10e 100644 --- a/src/drivers/barometer/ms5611/ms5611.cpp +++ b/src/drivers/barometer/ms5611/ms5611.cpp @@ -41,17 +41,24 @@ #include -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, enum MS56XX_DEVICE_TYPES device_type, - I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type), - _px4_barometer(interface->get_device_id()), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), _interface(interface), _prom(prom_buf.s), - _device_type(device_type), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { + switch (config.devid_driver_index) { + case DRV_BARO_DEVTYPE_MS5611: + _device_type = MS5611_DEVICE; + break; + + case DRV_BARO_DEVTYPE_MS5607: + default: + _device_type = MS5607_DEVICE; + break; + } } MS5611::~MS5611() @@ -100,15 +107,13 @@ MS5611::init() } /* state machine will have generated a report, copy it out */ - const sensor_baro_s &brp = _px4_barometer.get(); - if (_device_type == MS5607_DEVICE) { - if (brp.pressure < 520.0f) { + if (_last_pressure < 52'000.f) { /* This is likely not this device, abort */ ret = -EINVAL; break; - } else if (brp.pressure > 1500.0f) { + } else if (_last_pressure > 150'000.f) { /* This is likely not this device, abort */ ret = -EINVAL; break; @@ -121,12 +126,10 @@ MS5611::init() /* fall through */ case MS5611_DEVICE: _interface->set_device_type(DRV_BARO_DEVTYPE_MS5611); - _px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5611); break; case MS5607_DEVICE: _interface->set_device_type(DRV_BARO_DEVTYPE_MS5607); - _px4_barometer.set_device_type(DRV_BARO_DEVTYPE_MS5607); break; } @@ -136,6 +139,7 @@ MS5611::init() } if (ret == 0) { + _initialized = true; start(); } @@ -227,8 +231,6 @@ MS5611::measure() perf_count(_comms_errors); } - _px4_barometer.set_error_count(perf_event_count(_comms_errors)); - perf_end(_measure_perf); return ret; @@ -313,16 +315,26 @@ MS5611::collect() } } - float temperature = TEMP / 100.0f; - _px4_barometer.set_temperature(temperature); + _last_temperature = TEMP / 100.0f; } else { /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; - float pressure = P / 100.0f; /* convert to millibar */ + _last_pressure = P; + + // publish + if (_initialized && PX4_ISFINITE(_last_pressure) && PX4_ISFINITE(_last_temperature)) { + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = P; + sensor_baro.temperature = _last_temperature; + sensor_baro.error_count = perf_event_count(_comms_errors); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + } - _px4_barometer.update(timestamp_sample, pressure); } /* update the measurement state machine */ diff --git a/src/drivers/barometer/ms5611/ms5611.h b/src/drivers/barometer/ms5611/ms5611.h index e4d6ff5156c4..c8efaa7eca8b 100644 --- a/src/drivers/barometer/ms5611/ms5611.h +++ b/src/drivers/barometer/ms5611/ms5611.h @@ -39,17 +39,19 @@ #pragma once +#include "board_config.h" + #include -#include +#if defined(CONFIG_I2C) +# include +#endif // CONFIG_I2C + #include #include #include #include #include -#include - -#include "board_config.h" #define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ #define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ @@ -58,6 +60,10 @@ #define IOCTL_RESET 2 #define IOCTL_MEASURE 3 +#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ +#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ + + namespace ms5611 { @@ -92,7 +98,10 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency, spi_mode_e spi_mode); + +#if defined(CONFIG_I2C) extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum, int bus_frequency); +#endif // CONFIG_I2C typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint32_t devid, uint8_t busnum); diff --git a/src/drivers/barometer/ms5611/ms5611_i2c.cpp b/src/drivers/barometer/ms5611/ms5611_i2c.cpp index d5fbab39c331..30ddc54abb05 100644 --- a/src/drivers/barometer/ms5611/ms5611_i2c.cpp +++ b/src/drivers/barometer/ms5611/ms5611_i2c.cpp @@ -39,10 +39,9 @@ #include -#include "ms5611.h" +#if defined(CONFIG_I2C) -#define MS5611_ADDRESS_1 0x76 /* address select pins pulled high (PX4FMU series v1.6+) */ -#define MS5611_ADDRESS_2 0x77 /* address select pins pulled low (PX4FMU prototypes) */ +#include "ms5611.h" class MS5611_I2C : public device::I2C { @@ -144,18 +143,14 @@ MS5611_I2C::ioctl(unsigned operation, unsigned &arg) int MS5611_I2C::probe() { - _retries = 10; - if ((PX4_OK == _probe_address(MS5611_ADDRESS_1)) || (PX4_OK == _probe_address(MS5611_ADDRESS_2))) { - /* - * Disable retries; we may enable them selectively in some cases, - * but the device gets confused if we retry some of the commands. - */ - _retries = 0; + return PX4_OK; } + _retries = 1; + return -EIO; } @@ -186,7 +181,7 @@ MS5611_I2C::_reset() int result; /* bump the retry count */ - _retries = 10; + _retries = 3; result = transfer(&cmd, 1, nullptr, 0); _retries = old_retrycount; @@ -196,12 +191,6 @@ MS5611_I2C::_reset() int MS5611_I2C::_measure(unsigned addr) { - /* - * Disable retries on this command; we can't know whether failure - * means the device did or did not see the command. - */ - _retries = 0; - uint8_t cmd = addr; return transfer(&cmd, 1, nullptr, 0); } @@ -251,3 +240,5 @@ MS5611_I2C::_read_prom() /* calculate CRC and return success/failure accordingly */ return (ms5611::crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO; } + +#endif // CONFIG_I2C diff --git a/src/drivers/barometer/ms5611/ms5611_main.cpp b/src/drivers/barometer/ms5611/ms5611_main.cpp index 986c0fe3e187..1f19f6bfbac7 100644 --- a/src/drivers/barometer/ms5611/ms5611_main.cpp +++ b/src/drivers/barometer/ms5611/ms5611_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2019, 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 @@ -40,18 +40,21 @@ #include "MS5611.hpp" #include "ms5611.h" -I2CSPIDriverBase *MS5611::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *MS5611::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { ms5611::prom_u prom_buf; device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = MS5611_i2c_interface(prom_buf, iterator.devid(), iterator.bus(), cli.bus_frequency); +#if defined(CONFIG_I2C) - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = MS5611_spi_interface(prom_buf, iterator.devid(), iterator.bus(), cli.bus_frequency, cli.spi_mode); - } + if (config.bus_type == BOARD_I2C_BUS) { + interface = MS5611_i2c_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency); + + } else +#endif // CONFIG_I2C + if (config.bus_type == BOARD_SPI_BUS) { + interface = MS5611_spi_interface(prom_buf, config.spi_devid, config.bus, config.bus_frequency, config.spi_mode); + } if (interface == nullptr) { PX4_ERR("alloc failed"); @@ -60,12 +63,11 @@ I2CSPIDriverBase *MS5611::instantiate(const BusCLIArguments &cli, const BusInsta if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); return nullptr; } - MS5611 *dev = new MS5611(interface, prom_buf, (MS56XX_DEVICE_TYPES)cli.type, iterator.configuredBusOption(), - iterator.bus()); + MS5611 *dev = new MS5611(interface, prom_buf, config); if (dev == nullptr) { delete interface; @@ -85,7 +87,11 @@ void MS5611::print_usage() PRINT_MODULE_USAGE_NAME("ms5611", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("baro"); PRINT_MODULE_USAGE_COMMAND("start"); +#if defined(CONFIG_I2C) PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); +#else + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); +#endif PRINT_MODULE_USAGE_PARAM_STRING('T', "5611", "5607|5611", "Device type", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -94,23 +100,25 @@ extern "C" int ms5611_main(int argc, char *argv[]) { using ThisDriver = MS5611; int ch; - BusCLIArguments cli{true, true}; - cli.type = MS5611_DEVICE; +#if defined(CONFIG_I2C) + BusCLIArguments cli {true, true}; cli.default_i2c_frequency = 400000; - cli.default_spi_frequency = 20 * 1000 * 1000; + cli.i2c_address = MS5611_ADDRESS_1; +#else + BusCLIArguments cli {false, true}; +#endif + cli.default_spi_frequency = 16 * 1000 * 1000; uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5611; - while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) { switch (ch) { case 'T': { - int val = atoi(cli.optarg()); + int val = atoi(cli.optArg()); if (val == 5611) { - cli.type = MS5611_DEVICE; dev_type_driver = DRV_BARO_DEVTYPE_MS5611; } else if (val == 5607) { - cli.type = MS5607_DEVICE; dev_type_driver = DRV_BARO_DEVTYPE_MS5607; } } @@ -118,7 +126,7 @@ extern "C" int ms5611_main(int argc, char *argv[]) } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/barometer/ms5611/ms5611_spi.cpp b/src/drivers/barometer/ms5611/ms5611_spi.cpp index c0109fa1a39d..0125715afb41 100644 --- a/src/drivers/barometer/ms5611/ms5611_spi.cpp +++ b/src/drivers/barometer/ms5611/ms5611_spi.cpp @@ -115,27 +115,32 @@ MS5611_SPI::init() if (ret != OK) { PX4_DEBUG("SPI init failed"); - goto out; + return PX4_ERROR; } - /* send reset command */ - ret = _reset(); + // reset and read PROM (try up to 3 times) + for (int i = 0; i < 3; i++) { + /* send reset command */ + ret = _reset(); - if (ret != OK) { - PX4_DEBUG("reset failed"); - goto out; - } + if (ret != OK) { + PX4_DEBUG("reset failed"); + continue; + } - /* read PROM */ - ret = _read_prom(); + /* read PROM */ + ret = _read_prom(); - if (ret != OK) { - PX4_DEBUG("prom readout failed"); - goto out; + if (ret == OK) { + return PX4_OK; + + } else { + PX4_DEBUG("prom readout failed"); + continue; + } } -out: - return ret; + return PX4_ERROR; } int diff --git a/src/drivers/barometer/ms5837/CMakeLists.txt b/src/drivers/barometer/ms5837/CMakeLists.txt new file mode 100644 index 000000000000..027d0484081d --- /dev/null +++ b/src/drivers/barometer/ms5837/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__barometer__ms5837 + MAIN ms5837 + COMPILE_FLAGS + SRCS + ms5837_main.cpp + ms5837_registers.h + MS5837.cpp + MS5837.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/ms5837/Kconfig b/src/drivers/barometer/ms5837/Kconfig new file mode 100644 index 000000000000..8d71727de622 --- /dev/null +++ b/src/drivers/barometer/ms5837/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_MS5837 + bool "ms5837" + default n + ---help--- + Enable support for ms5837 diff --git a/src/drivers/barometer/ms5837/MS5837.cpp b/src/drivers/barometer/ms5837/MS5837.cpp new file mode 100644 index 000000000000..7041d886d937 --- /dev/null +++ b/src/drivers/barometer/ms5837/MS5837.cpp @@ -0,0 +1,440 @@ +/**************************************************************************** + * + * 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 ms5837.cpp + * Driver for the MS5837 barometric pressure sensor connected via I2C. + */ + +#include "MS5837.hpp" + +MS5837::MS5837(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) +{ +} + +MS5837::~MS5837() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); +} + +int MS5837::init() +{ + + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + /* do a first measurement cycle to populate reports with valid data */ + _measure_phase = 0; + + while (true) { + /* do temperature first */ + if (OK != _measure()) { + ret = -EIO; + break; + } + + px4_usleep(MS5837_CONVERSION_INTERVAL); + + if (OK != _collect()) { + ret = -EIO; + break; + } + + /* now do a pressure measurement */ + if (OK != _measure()) { + ret = -EIO; + break; + } + + px4_usleep(MS5837_CONVERSION_INTERVAL); + + if (OK != _collect()) { + ret = -EIO; + break; + } + + set_device_type(DRV_BARO_DEVTYPE_MS5837); + + ret = OK; + + break; + + } + + if (ret == 0) { + _start(); + } + + return ret; +} + +int MS5837::_reset() +{ + unsigned old_retrycount = _retries; + uint8_t cmd = MS5837_RESET; + + /* bump the retry count */ + _retries = 3; + int result = transfer(&cmd, 1, nullptr, 0); + _retries = old_retrycount; + + return result; +} + +int MS5837::probe() +{ + if ((PX4_OK == _probe_address(MS5837_ADDRESS))) { + + return PX4_OK; + } + + _retries = 1; + + return -EIO; +} + +int MS5837::_probe_address(uint8_t address) +{ + /* select the address we are going to try */ + set_device_address(address); + + /* send reset command */ + if (PX4_OK != _reset()) { + return -EIO; + } + + /* read PROM */ + if (PX4_OK != _read_prom()) { + return -EIO; + } + + return PX4_OK; +} + +int MS5837::read(unsigned offset, void *data, unsigned count) +{ + union _cvt { + uint8_t b[4]; + uint32_t w; + } *cvt = (_cvt *)data; + uint8_t buf[3]; + + /* read the most recent measurement */ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, &buf[0], 3); + + if (ret == PX4_OK) { + /* fetch the raw value */ + cvt->b[0] = buf[2]; + cvt->b[1] = buf[1]; + cvt->b[2] = buf[0]; + cvt->b[3] = 0; + } + + return ret; +} + +void MS5837::RunImpl() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = _collect(); + + if (ret != OK) { + if (ret == -6) { + /* + * The ms5837 seems to regularly fail to respond to + * its address; this happens often enough that we'd rather not + * spam the console with a message for this. + */ + } else { + //DEVICE_LOG("collection error %d", ret); + } + + /* issue a reset command to the sensor */ + _reset(); + /* reset the collection state machine and try again - we need + * to wait 2.8 ms after issuing the sensor reset command + * according to the MS5837 datasheet + */ + ScheduleDelayed(2800); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + } + + /* measurement phase */ + ret = _measure(); + + if (ret != OK) { + /* issue a reset command to the sensor */ + _reset(); + /* reset the collection state machine and try again */ + _start(); + return; + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(MS5837_CONVERSION_INTERVAL); +} + +void MS5837::_start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _measure_phase = 0; + + /* schedule a cycle to start things */ + ScheduleDelayed(MS5837_CONVERSION_INTERVAL); +} + +int MS5837::_measure() +{ + perf_begin(_measure_perf); + + /* + * In phase zero, request temperature; in other phases, request pressure. + */ + unsigned addr = (_measure_phase == 0) ? ADDR_CMD_CONVERT_D2 : ADDR_CMD_CONVERT_D1; + + /* + * Send the command to begin measuring. + */ + uint8_t cmd = addr; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + perf_end(_measure_perf); + + return ret; +} + +int MS5837::_collect() +{ + uint32_t raw; + + perf_begin(_sample_perf); + + /* read the most recent measurement - read offset/size are hardcoded in the interface */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = read(0, (void *)&raw, 0); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + /* handle a measurement */ + if (_measure_phase == 0) { + + /* temperature offset (in ADC units) */ + int32_t dT = (int32_t)raw - ((int32_t)_prom.s.c5_reference_temp << 8); + + /* absolute temperature in centidegrees - note intermediate value is outside 32-bit range */ + int32_t TEMP = 2000 + (int32_t)(((int64_t)dT * _prom.s.c6_temp_coeff_temp) >> 23); + + /* base sensor scale/offset values */ + + /* Perform MS5837 Caculation */ + _OFF = ((int64_t)_prom.s.c2_pressure_offset << 16) + (((int64_t)_prom.s.c4_temp_coeff_pres_offset * dT) >> 7); + _SENS = ((int64_t)_prom.s.c1_pressure_sens << 15) + (((int64_t)_prom.s.c3_temp_coeff_pres_sens * dT) >> 8); + + /* MS5837 temperature compensation */ + int64_t T2 = 0; + + int64_t f = 0; + int64_t OFF2 = 0; + int64_t SENS2 = 0; + + if (TEMP < 2000) { + + T2 = 3 * ((int64_t)POW2(dT) >> 33); + + f = POW2((int64_t)TEMP - 2000); + OFF2 = 3 * f >> 1; + SENS2 = 5 * f >> 3; + + if (TEMP < -1500) { + + int64_t f2 = POW2(TEMP + 1500); + OFF2 += 7 * f2; + SENS2 += f2 << 2; + } + + } else if (TEMP >= 2000) { + T2 = 2 * ((int64_t)POW2(dT) >> 37); + + f = POW2((int64_t)TEMP - 2000); + OFF2 = 1 * f >> 4; + SENS2 = 0; + } + + TEMP -= (int32_t)T2; + _OFF -= OFF2; + _SENS -= SENS2; + + _last_temperature = TEMP / 100.0f; + + } else { + /* pressure calculation, result in Pa */ + int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 13; + + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = P; + sensor_baro.temperature = T; + sensor_baro.error_count = perf_event_count(_comms_errors); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + } + + /* update the measurement state machine */ + INCREMENT(_measure_phase, MS5837_MEASUREMENT_RATIO + 1); + + perf_end(_sample_perf); + + return OK; +} + +void MS5837::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int MS5837::_read_prom() +{ + uint8_t prom_buf[2]; + union { + uint8_t b[2]; + uint16_t w; + } cvt; + + /* + * Wait for PROM contents to be in the device (2.8 ms) in the case we are + * called immediately after reset. + */ + px4_usleep(3000); + + uint8_t last_val = 0; + bool bits_stuck = true; + + /* read and convert PROM words */ + for (int i = 0; i < 7; i++) { + uint8_t cmd = MS5837_PROM_READ + (i * 2); + + if (PX4_OK != transfer(&cmd, 1, &prom_buf[0], 2)) { + break; + } + + /* check if all bytes are zero */ + if (i == 0) { + /* initialize to first byte read */ + last_val = prom_buf[0]; + } + + if ((prom_buf[0] != last_val) || (prom_buf[1] != last_val)) { + bits_stuck = false; + } + + /* assemble 16 bit value and convert from big endian (sensor) to little endian (MCU) */ + cvt.b[0] = prom_buf[1]; + cvt.b[1] = prom_buf[0]; + _prom.c[i] = cvt.w; + } + + /* calculate CRC and return success/failure accordingly */ + return (_crc4(&_prom.c[0]) && !bits_stuck) ? PX4_OK : -EIO; +} + +/** + * MS5837 crc4 cribbed from the datasheet + */ +bool MS5837::_crc4(uint16_t *n_prom) +{ + uint16_t n_rem = 0; + uint16_t crcRead = n_prom[0] >> 12; + n_prom[0] = ((n_prom[0]) & 0x0FFF); + n_prom[7] = 0; + + for (uint8_t i = 0 ; i < 16; i++) { + if (i % 2 == 1) { + n_rem ^= (uint16_t)((n_prom[i >> 1]) & 0x00FF); + + } else { + n_rem ^= (uint16_t)(n_prom[i >> 1] >> 8); + } + + for (uint8_t n_bit = 8 ; n_bit > 0 ; n_bit--) { + if (n_rem & 0x8000) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + n_rem = ((n_rem >> 12) & 0x000F); + + return (n_rem ^ 0x00) == crcRead; +} diff --git a/src/drivers/barometer/ms5837/MS5837.hpp b/src/drivers/barometer/ms5837/MS5837.hpp new file mode 100644 index 000000000000..4de1c38462c6 --- /dev/null +++ b/src/drivers/barometer/ms5837/MS5837.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ms5837_registers.h" + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) + +/* helper macro for arithmetic - returns the square of the argument */ +#define POW2(_x) ((_x) * (_x)) + +class MS5837 : public device::I2C, public I2CSPIDriver +{ +public: + MS5837(const I2CSPIDriverConfig &config); + ~MS5837() override; + + static void print_usage(); + + int init(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void RunImpl(); + void print_status() override; + int read(unsigned offset, void *data, unsigned count) override; + +private: + int probe() override; + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + + ms5837::prom_u _prom{}; + + bool _collect_phase{false}; + unsigned _measure_phase{false}; + + /* intermediate temperature values per MS5611/MS5607 datasheet */ + int64_t _OFF{0}; + int64_t _SENS{0}; + + float _last_temperature{NAN}; + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + /** + * Initialize the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void _start(); + + /** + * Issue a measurement command for the current state. + * + * @return OK if the measurement command was successful. + */ + int _measure(); + + /** + * Collect the result of the most recent measurement. + */ + int _collect(); + + int _probe_address(uint8_t address); + + /** + * Send a reset command to the MS5837. + * + * This is required after any bus reset. + */ + int _reset(); + + /** + * Read the MS5837 PROM + * + * @return PX4_OK if the PROM reads successfully. + */ + int _read_prom(); + + bool _crc4(uint16_t *n_prom); +}; diff --git a/src/drivers/barometer/ms5837/ms5837_main.cpp b/src/drivers/barometer/ms5837/ms5837_main.cpp new file mode 100644 index 000000000000..ef6084c547e2 --- /dev/null +++ b/src/drivers/barometer/ms5837/ms5837_main.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 + +#include "MS5837.hpp" + +void MS5837::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms5837", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms5837_main(int argc, char *argv[]) +{ + using ThisDriver = MS5837; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + uint16_t dev_type_driver = DRV_BARO_DEVTYPE_MS5837; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + cli.i2c_address = MS5837_ADDRESS; + + BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver); + + 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/barometer/ms5837/ms5837_registers.h b/src/drivers/barometer/ms5837/ms5837_registers.h new file mode 100644 index 000000000000..75ef7a6ddb12 --- /dev/null +++ b/src/drivers/barometer/ms5837/ms5837_registers.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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 ms5837_registers.h + * + * Shared defines for the ms5837 driver. + */ + +#pragma once + +#include +#include "board_config.h" + +/* interface ioctls */ +#define IOCTL_RESET 2 +#define IOCTL_MEASURE 3 + +#define MS5837_ADDRESS 0x76 + +#define MS5837_RESET 0x1E +#define MS5837_ADC_READ 0x00 +#define MS5837_PROM_READ 0xA0 + +#define MS5837_30BA26 0x1A // Sensor version: From MS5837_30BA datasheet Version PROM Word 0 + +/* + * MS5837 internal constants and data structures. + */ +#define ADDR_CMD_CONVERT_D1_OSR256 0x40 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR512 0x42 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR1024 0x44 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR2048 0x46 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D1_OSR4096 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2_OSR256 0x50 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR512 0x52 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR1024 0x54 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR2048 0x56 /* write to this address to start temperature conversion */ +#define ADDR_CMD_CONVERT_D2_OSR4096 0x58 /* write to this address to start temperature conversion */ + +/* + use an OSR of 1024 to reduce the self-heating effect of the + sensor. Information from MS tells us that some individual sensors + are quite sensitive to this effect and that reducing the OSR can + make a big difference + */ +#define ADDR_CMD_CONVERT_D1 ADDR_CMD_CONVERT_D1_OSR1024 +#define ADDR_CMD_CONVERT_D2 ADDR_CMD_CONVERT_D2_OSR1024 + +/* + * Maximum internal conversion time for OSR 1024 is 2.28 ms. We set an update + * rate of 100Hz which is be very safe not to read the ADC before the + * conversion finished + */ +#define MS5837_CONVERSION_INTERVAL 10000 /* microseconds */ +#define MS5837_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ + +namespace ms5837 +{ + +/** + * Calibration PROM as reported by the device. + */ +#pragma pack(push,1) +struct prom_s { + uint16_t serial_and_crc; + uint16_t c1_pressure_sens; + uint16_t c2_pressure_offset; + uint16_t c3_temp_coeff_pres_sens; + uint16_t c4_temp_coeff_pres_offset; + uint16_t c5_reference_temp; + uint16_t c6_temp_coeff_temp; + uint16_t factory_setup; +}; + +/** + * Grody hack for crc4() + */ +union prom_u { + uint16_t c[8]; + prom_s s; +}; +#pragma pack(pop) + +} /* namespace */ diff --git a/src/drivers/barometer/tcbp001ta/CMakeLists.txt b/src/drivers/barometer/tcbp001ta/CMakeLists.txt index 56a8badfe0da..4af09cb91a58 100644 --- a/src/drivers/barometer/tcbp001ta/CMakeLists.txt +++ b/src/drivers/barometer/tcbp001ta/CMakeLists.txt @@ -39,6 +39,5 @@ px4_add_module( tcbp001ta_spi.cpp tcbp001ta_main.cpp DEPENDS - drivers_barometer px4_work_queue ) diff --git a/src/drivers/barometer/tcbp001ta/Kconfig b/src/drivers/barometer/tcbp001ta/Kconfig new file mode 100644 index 000000000000..ea251c373668 --- /dev/null +++ b/src/drivers/barometer/tcbp001ta/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_TCBP001TA + bool "tcbp001ta" + default n + ---help--- + Enable support for tcbp001ta \ No newline at end of file diff --git a/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp b/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp index a04f9ed93951..c903a910b73b 100644 --- a/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp +++ b/src/drivers/barometer/tcbp001ta/tcbp001ta.cpp @@ -45,13 +45,11 @@ TCBP001TA::TCBP001TA(tcbp001ta::ITCBP001TA *interface) : ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), - _px4_baro(interface->get_device_id()), _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")) { - _px4_baro.set_device_type(DRV_BARO_DEVTYPE_TCBP001TA); } TCBP001TA::~TCBP001TA() @@ -252,12 +250,15 @@ TCBP001TA::collect() Praw_sc * _fcal.c01 + Praw_sc * Praw_sc * (_fcal.c11 + Praw_sc * _fcal.c21); - _px4_baro.set_error_count(perf_event_count(_comms_errors)); - _px4_baro.set_temperature(T); - - float pressure = P / 100.0f; // to mbar - // PX4_INFO("press %f", double(pressure)); - _px4_baro.update(timestamp_sample, pressure); + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = P; + sensor_baro.temperature = T; + 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); @@ -270,8 +271,6 @@ TCBP001TA::print_info() perf_print_counter(_sample_perf); perf_print_counter(_measure_perf); perf_print_counter(_comms_errors); - - _px4_baro.print_status(); } uint32_t diff --git a/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp b/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp index c736e05fa664..1abe63ea177e 100644 --- a/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp +++ b/src/drivers/barometer/tcbp001ta/tcbp001ta.hpp @@ -38,7 +38,8 @@ #include #include #include -#include +#include +#include #include class TCBP001TA : public px4::ScheduledWorkItem @@ -63,7 +64,7 @@ class TCBP001TA : public px4::ScheduledWorkItem int measure(); //start measure int collect(); //get results and publish - PX4Barometer _px4_baro; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; tcbp001ta::ITCBP001TA *_interface; diff --git a/src/drivers/barometer/tcbp001ta/tcbp001ta_spi.cpp b/src/drivers/barometer/tcbp001ta/tcbp001ta_spi.cpp index 66580c5b8777..5a5225563cbf 100644 --- a/src/drivers/barometer/tcbp001ta/tcbp001ta_spi.cpp +++ b/src/drivers/barometer/tcbp001ta/tcbp001ta_spi.cpp @@ -93,6 +93,7 @@ tcbp001ta_spi_interface(uint8_t busnum, uint32_t device) TCBP001TA_SPI::TCBP001TA_SPI(uint8_t bus, uint32_t device) : SPI("TCBP001TA_SPI", nullptr, bus, device, SPIDEV_MODE3, 10 * 1000 * 1000) { + set_device_type(DRV_BARO_DEVTYPE_TCBP001TA); } uint8_t diff --git a/src/drivers/batt_smbus/Kconfig b/src/drivers/batt_smbus/Kconfig new file mode 100644 index 000000000000..0b94c11e5535 --- /dev/null +++ b/src/drivers/batt_smbus/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BATT_SMBUS + bool "batt_smbus" + default n + ---help--- + Enable support for batt_smbus \ No newline at end of file diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 8952a2c638c7..1dfac055b48d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -47,16 +47,15 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); -BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, - interface->get_device_address()), +BATT_SMBUS::BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface) : + I2CSPIDriver(config), _interface(interface) { - int battsource = 1; - int batt_device_type = (int)SMBUS_DEVICE_TYPE::UNDEFINED; + int32_t battsource = 1; + int32_t batt_device_type = static_cast(SMBUS_DEVICE_TYPE::UNDEFINED); - param_set(param_find("BAT_SOURCE"), &battsource); - param_get(param_find("BAT_SMBUS_MODEL"), &batt_device_type); + param_set(param_find("BAT1_SOURCE"), &battsource); + param_get(param_find("BAT1_SMBUS_MODEL"), &batt_device_type); //TODO: probe the device and autodetect its type @@ -84,8 +83,8 @@ BATT_SMBUS::~BATT_SMBUS() delete _interface; } - int battsource = 0; - param_set(param_find("BAT_SOURCE"), &battsource); + int32_t battsource = 0; + param_set(param_find("BAT1_SOURCE"), &battsource); } void BATT_SMBUS::RunImpl() @@ -129,7 +128,7 @@ void BATT_SMBUS::RunImpl() float average_current = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult; - new_report.average_current_a = average_current; + new_report.current_average_a = average_current; // If current is high, turn under voltage protection off. This is neccessary to prevent // a battery from cutting off while flying with high current near the end of the packs capacity. @@ -137,7 +136,7 @@ void BATT_SMBUS::RunImpl() // Read run time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_RUN_TIME_TO_EMPTY, result); - new_report.run_time_to_empty = result; + new_report.time_remaining_s = result * 60; // Read average time to empty (minutes). ret |= _interface->read_word(BATT_SMBUS_AVERAGE_TIME_TO_EMPTY, result); @@ -360,13 +359,14 @@ int BATT_SMBUS::get_startup_info() int ret = PX4_OK; // Read battery threshold params on startup. + // TODO: support instances param_get(param_find("BAT_CRIT_THR"), &_crit_thr); param_get(param_find("BAT_LOW_THR"), &_low_thr); param_get(param_find("BAT_EMERGEN_THR"), &_emergency_thr); - param_get(param_find("BAT_C_MULT"), &_c_mult); + param_get(param_find("BAT1_C_MULT"), &_c_mult); int32_t cell_count_param = 0; - param_get(param_find("BAT_N_CELLS"), &cell_count_param); + param_get(param_find("BAT1_N_CELLS"), &cell_count_param); _cell_count = math::min((uint8_t)cell_count_param, MAX_NUM_OF_CELLS); ret |= _interface->block_read(BATT_SMBUS_MANUFACTURER_NAME, _manufacturer_name, BATT_SMBUS_MANUFACTURER_NAME_SIZE, @@ -534,15 +534,14 @@ To write to flash to set parameters. address, number_of_bytes, byte0, ... , byte PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *BATT_SMBUS::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *BATT_SMBUS::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { - SMBus *interface = new SMBus(iterator.bus(), cli.i2c_address); + SMBus *interface = new SMBus(DRV_BAT_DEVTYPE_SMBUS, config.bus, config.i2c_address); if (interface == nullptr) { PX4_ERR("alloc failed"); return nullptr; } - BATT_SMBUS *instance = new BATT_SMBUS(iterator.configuredBusOption(), iterator.bus(), interface); + BATT_SMBUS *instance = new BATT_SMBUS(config, interface); if (instance == nullptr) { PX4_ERR("alloc failed"); @@ -567,8 +566,8 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli) switch(cli.custom1) { case 1: { PX4_INFO("The manufacturer name: %s", _manufacturer_name); - PX4_INFO("The manufacturer date: %d", _manufacture_date); - PX4_INFO("The serial number: %d", _serial_number); + PX4_INFO("The manufacturer date: %" PRId16, _manufacture_date); + PX4_INFO("The serial number: %d" PRId16, _serial_number); } break; case 2: @@ -590,7 +589,7 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli) unsigned length = tx_buf[0]; if (PX4_OK != dataflash_write(address, tx_buf+1, length)) { - PX4_ERR("Dataflash write failed: %d", address); + PX4_ERR("Dataflash write failed: %u", address); } px4_usleep(100_ms); } diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index 1babc9c47fc0..1915865c80fb 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -44,7 +44,7 @@ #pragma once -#include +#include #include #include #include @@ -118,22 +118,20 @@ using namespace time_literals; #define BATT_SMBUS_ENABLED_PROTECTIONS_A_DEFAULT 0xcf #define BATT_SMBUS_ENABLED_PROTECTIONS_A_CUV_DISABLED 0xce - enum class SMBUS_DEVICE_TYPE { - UNDEFINED = 0, - BQ40Z50 = 1, - BQ40Z80 = 2, + UNDEFINED = 0, + BQ40Z50 = 1, + BQ40Z80 = 2, }; class BATT_SMBUS : public I2CSPIDriver { public: - BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface); + BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface); ~BATT_SMBUS(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); friend SMBus; diff --git a/src/drivers/batt_smbus/parameters.c b/src/drivers/batt_smbus/parameters.c index dd3c9d22bf2c..0d256c757724 100644 --- a/src/drivers/batt_smbus/parameters.c +++ b/src/drivers/batt_smbus/parameters.c @@ -48,7 +48,7 @@ PARAM_DEFINE_INT32(SENS_EN_BATT, 0); * @decimal 1 * @group Sensors */ -PARAM_DEFINE_FLOAT(BAT_C_MULT, 1.0f); +PARAM_DEFINE_FLOAT(BAT1_C_MULT, 1.0f); /** * Battery device model @@ -61,4 +61,4 @@ PARAM_DEFINE_FLOAT(BAT_C_MULT, 1.0f); * @value 1 BQ40Z50 based * @value 2 BQ40Z80 based */ -PARAM_DEFINE_INT32(BAT_SMBUS_MODEL, 0); +PARAM_DEFINE_INT32(BAT1_SMBUS_MODEL, 0); diff --git a/src/drivers/bootloaders/CMakeLists.txt b/src/drivers/bootloaders/CMakeLists.txt index 8472da88363f..32f514a36a39 100644 --- a/src/drivers/bootloaders/CMakeLists.txt +++ b/src/drivers/bootloaders/CMakeLists.txt @@ -47,6 +47,9 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES "canbootloader") set(HW_MINOR ${uavcanblid_hw_version_minor}) set(SW_MAJOR ${uavcanblid_sw_version_major}) set(SW_MINOR ${uavcanblid_sw_version_minor}) + if("${uavcanbl_padding}" STREQUAL "") + set(uavcanbl_padding 4) + endif() math(EXPR HWBOARD_ID "(${HW_MAJOR} << 8) + ${HW_MINOR}") @@ -68,13 +71,13 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES "canbootloader") ${PX4_BINARY_DIR}/${uavcan_bl_image_name} ${PX4_BINARY_DIR}/deploy/${HWBOARD_ID}.bin COMMAND - ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --use-git-hash ${PX4_BOARD}.bin ${uavcan_bl_image_name} + ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_CONFIG}.bin ${uavcan_bl_image_name} COMMAND COMMAND ${CMAKE_COMMAND} -E make_directory deploy COMMAND ${CMAKE_COMMAND} -E copy ${uavcan_bl_image_name} deploy/${HWBOARD_ID}.bin DEPENDS - ${PX4_BINARY_DIR}/${PX4_BOARD}.bin + ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py WORKING_DIRECTORY ${PX4_BINARY_DIR} diff --git a/src/drivers/bootloaders/Kconfig b/src/drivers/bootloaders/Kconfig new file mode 100644 index 000000000000..116e37539037 --- /dev/null +++ b/src/drivers/bootloaders/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BOOTLOADERS + bool "bootloaders" + default n + ---help--- + Enable support for bootloaders \ No newline at end of file diff --git a/src/drivers/bootloaders/make_can_boot_descriptor.py b/src/drivers/bootloaders/make_can_boot_descriptor.py index a0a52561421c..f549de6f8f6e 100755 --- a/src/drivers/bootloaders/make_can_boot_descriptor.py +++ b/src/drivers/bootloaders/make_can_boot_descriptor.py @@ -120,18 +120,17 @@ def valid(self): class FirmwareImage(object): - def __init__(self, path_or_file, mode="r"): + def __init__(self, path_or_file, mode="r", padding = 4): + self._padding = padding; if getattr(path_or_file, "read", None): self._file = path_or_file self._do_close = False - self._padding = 0 else: if "b" not in mode: self._file = open(path_or_file, mode + "b") else: self._file = open(path_or_file, mode) self._do_close = True - self._padding = 4 if "r" in mode: self._contents = BytesIO(self._file.read()) @@ -216,11 +215,20 @@ def length(self): prev_offset = self._contents.tell() self._contents.seek(0, os.SEEK_END) self._length = self._contents.tell() - if self._padding: - fill = self._padding - (self._length % self._padding) - if fill: - self._length += fill - self._padding = fill + # Was Padding requested + if self._padding != 0: + # If so. then is the file already a multiple of the padding? + if 0 == (self._length % self._padding): + # padding not needed + self._padding = 0 + + # Still need padding + if self._padding: + fill = self._padding - (self._length % self._padding) + if fill: + self._length += fill + #report back the fill which is the padding + self._padding = fill self._contents.seek(prev_offset) return self._length @@ -285,6 +293,8 @@ def app_descriptor(self, value): metavar="IMAGE") parser.add_option("-v", "--verbose", dest="verbose", action="store_true", help="show additional firmware information on stdout") + parser.add_option("-p", "--padding", dest="padding", type= int, default=4, + help="Padds the image to a multple of") options, args = parser.parse_args() if len(args) not in (0, 2): @@ -315,8 +325,8 @@ def app_descriptor(self, value): bootloader_size = int(options.bootloader_size) - with FirmwareImage(in_file, "rb") as in_image: - with FirmwareImage(out_file, "wb") as out_image: + with FirmwareImage(in_file, "rb", 0) as in_image: + with FirmwareImage(out_file, "wb", options.padding) as out_image: image = in_image.read() out_image.write(bootloader_image) out_image.write(image[bootloader_size:]) diff --git a/src/drivers/camera_capture/CMakeLists.txt b/src/drivers/camera_capture/CMakeLists.txt index 44016de5860b..37e9e88b86eb 100644 --- a/src/drivers/camera_capture/CMakeLists.txt +++ b/src/drivers/camera_capture/CMakeLists.txt @@ -30,11 +30,20 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + px4_add_module( MODULE drivers__camera_capture MAIN camera_capture COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" SRCS camera_capture.cpp - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : + DEPENDS + arch_io_pins +) diff --git a/src/drivers/camera_capture/Kconfig b/src/drivers/camera_capture/Kconfig new file mode 100644 index 000000000000..c8133484656e --- /dev/null +++ b/src/drivers/camera_capture/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_CAMERA_CAPTURE + bool "camera_capture" + default n + ---help--- + Enable support for camera_capture \ No newline at end of file diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 6c350ddc237a..7d1183b2dfdf 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -41,6 +41,8 @@ #include "camera_capture.hpp" +#include + #define commandParamToInt(n) static_cast(n >= 0 ? n + 0.5f : n - 0.5f) namespace camera_capture @@ -64,6 +66,34 @@ 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 + param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); + int32_t ctrl_alloc = 0; + + if (p_ctrl_alloc != PARAM_INVALID) { + param_get(p_ctrl_alloc, &ctrl_alloc); + } + + if (ctrl_alloc == 1) { + _capture_channel = -1; + + for (unsigned i = 0; i < 16 && _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 + _capture_channel = i; + } + } + } + } + + _trigger_pub.advertise(); } CameraCapture::~CameraCapture() @@ -75,7 +105,7 @@ void CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { _trigger.chan_index = chan_index; - _trigger.edge_time = edge_time; + _trigger.hrt_edge_time = edge_time; _trigger.edge_state = edge_state; _trigger.overflow = overflow; @@ -88,7 +118,7 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) CameraCapture *dev = static_cast(arg); dev->_trigger.chan_index = 0; - dev->_trigger.edge_time = hrt_absolute_time(); + dev->_trigger.hrt_edge_time = hrt_absolute_time(); dev->_trigger.edge_state = 0; dev->_trigger.overflow = 0; @@ -114,19 +144,20 @@ CameraCapture::publish_trigger() // MODES 1 and 2 are not fully tested if (_camera_capture_mode == 0 || _gpio_capture) { - trigger.timestamp = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + trigger.timestamp = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); trigger.seq = _capture_seq++; _last_trig_time = trigger.timestamp; + publish = true; } else if (_camera_capture_mode == 1) { // Get timestamp of mid-exposure (active high) if (_trigger.edge_state == 1) { - _last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); } else if (_trigger.edge_state == 0 && _last_trig_begin_time > 0) { - trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2); + trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2); trigger.seq = _capture_seq++; - _last_exposure_time = _trigger.edge_time - _last_trig_begin_time; + _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time; _last_trig_time = trigger.timestamp; publish = true; _capture_seq++; @@ -134,12 +165,12 @@ CameraCapture::publish_trigger() } else { // Get timestamp of mid-exposure (active low) if (_trigger.edge_state == 0) { - _last_trig_begin_time = _trigger.edge_time - uint64_t(1000 * _strobe_delay); + _last_trig_begin_time = _trigger.hrt_edge_time - uint64_t(1000 * _strobe_delay); } else if (_trigger.edge_state == 1 && _last_trig_begin_time > 0) { - trigger.timestamp = _trigger.edge_time - ((_trigger.edge_time - _last_trig_begin_time) / 2); + trigger.timestamp = _trigger.hrt_edge_time - ((_trigger.hrt_edge_time - _last_trig_begin_time) / 2); trigger.seq = _capture_seq++; - _last_exposure_time = _trigger.edge_time - _last_trig_begin_time; + _last_exposure_time = _trigger.hrt_edge_time - _last_trig_begin_time; _last_trig_time = trigger.timestamp; publish = true; } @@ -153,6 +184,25 @@ CameraCapture::publish_trigger() return; } + pps_capture_s pps_capture; + + if (_pps_capture_sub.update(&pps_capture)) { + _pps_hrt_timestamp = pps_capture.timestamp; + _pps_rtc_timestamp = pps_capture.rtc_timestamp; + } + + + if (_pps_hrt_timestamp > 0) { + // Last PPS RTC time + elapsed time to the camera capture interrupt + trigger.timestamp_utc = _pps_rtc_timestamp + (trigger.timestamp - _pps_hrt_timestamp); + + } else { + // No PPS capture received, use RTC clock as fallback + timespec tv{}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + trigger.timestamp_utc = ts_to_abstime(&tv) - hrt_elapsed_time(&trigger.timestamp); + } + _trigger_pub.publish(trigger); } @@ -206,74 +256,47 @@ CameraCapture::Run() void CameraCapture::set_capture_control(bool enabled) { -// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin +// a board can define BOARD_CAPTURE_GPIO to use a separate capture pin. It's used if no channel is configured #if defined(BOARD_CAPTURE_GPIO) - px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &CameraCapture::gpio_interrupt_routine, this); - _capture_enabled = enabled; - _gpio_capture = true; - reset_statistics(false); - -#else - - int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); - - if (fd < 0) { - PX4_ERR("open fail"); - return; - } - - input_capture_config_t conf; - conf.channel = 5; // FMU chan 6 - conf.filter = 0; - - if (_camera_capture_mode == 0) { - conf.edge = _camera_capture_edge ? Rising : Falling; - - } else { - conf.edge = Both; + if (_capture_channel == -1) { + px4_arch_gpiosetevent(BOARD_CAPTURE_GPIO, true, false, true, &CameraCapture::gpio_interrupt_routine, this); + _capture_enabled = enabled; + _gpio_capture = true; + reset_statistics(false); } - conf.callback = nullptr; - conf.context = nullptr; - - if (enabled) { +#endif - conf.callback = &CameraCapture::capture_trampoline; - conf.context = this; + if (!_gpio_capture) { + if (_capture_channel == -1) { + PX4_WARN("No capture channel configured"); + _capture_enabled = false; - unsigned int capture_count = 0; + } else { + capture_callback_t callback = nullptr; + void *context = nullptr; - if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { - PX4_INFO("Not in a capture mode"); + if (enabled) { + callback = &CameraCapture::capture_trampoline; + context = this; + } - unsigned long mode = PWM_SERVO_MODE_4PWM2CAP; + int ret = up_input_capture_set_callback(_capture_channel, callback, context); - if (::ioctl(fd, PWM_SERVO_SET_MODE, mode) == 0) { - PX4_INFO("Mode changed to 4PWM2CAP"); + if (ret == 0) { + _capture_enabled = enabled; + _gpio_capture = false; } else { - PX4_ERR("Mode NOT changed to 4PWM2CAP!"); - goto err_out; + PX4_ERR("Unable to set capture callback for chan %" PRIu8 " (%i)", _capture_channel, ret); + _capture_enabled = false; } - } - } - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { - _capture_enabled = enabled; - _gpio_capture = false; - - } else { - PX4_ERR("Unable to set capture callback for chan %u\n", conf.channel); - _capture_enabled = false; - goto err_out; + reset_statistics(false); + } } - reset_statistics(false); - -err_out: - ::close(fd); -#endif } void @@ -292,6 +315,21 @@ CameraCapture::reset_statistics(bool reset_seq) int CameraCapture::start() { + if (!_gpio_capture && _capture_channel != -1) { + input_capture_edge edge = Both; + + if (_camera_capture_mode == 0) { + edge = _camera_capture_edge ? Rising : Falling; + } + + int ret = up_input_capture_set(_capture_channel, edge, 0, nullptr, nullptr); + + if (ret != 0) { + PX4_ERR("up_input_capture_set failed (%i)", ret); + return ret; + } + } + // run every 100 ms (10 Hz) ScheduleOnInterval(100000, 10000); @@ -314,7 +352,7 @@ void CameraCapture::status() { PX4_INFO("Capture enabled : %s", _capture_enabled ? "YES" : "NO"); - PX4_INFO("Frame sequence : %u", _capture_seq); + PX4_INFO("Frame sequence : %" PRIu32, _capture_seq); if (_last_trig_time != 0) { PX4_INFO("Last trigger timestamp : %" PRIu64 " (%i ms ago)", _last_trig_time, @@ -328,7 +366,32 @@ CameraCapture::status() PX4_INFO("Last exposure time : %0.2f ms", double(_last_exposure_time) / 1000.0); } - PX4_INFO("Number of overflows : %u", _capture_overflows); + PX4_INFO("Number of overflows : %" PRIu32, _capture_overflows); + + if (_gpio_capture) { + PX4_INFO("Using board GPIO pin"); + + } else if (_capture_channel == -1) { + PX4_INFO("No Capture channel configured"); + + } else { + input_capture_stats_t stats; + int ret = up_input_capture_get_stats(_capture_channel, &stats, false); + + if (ret != 0) { + PX4_ERR("Unable to get stats for chan %" PRIu8 " (%i)", _capture_channel, ret); + + } else { + PX4_INFO("Status chan: %" PRIu8 " edges: %" PRIu32 " last time: %" PRIu64 " last state: %" PRIu32 + " overflows: %" PRIu32 " latency: %" PRIu16, + _capture_channel, + stats.edges, + stats.last_time, + stats.last_edge, + stats.overflows, + stats.latency); + } + } } static int usage() diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 2bdb31761e02..baeebb21a3f1 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -51,12 +51,10 @@ #include #include #include +#include #include #include -#define PX4FMU_DEVICE_PATH "/dev/px4fmu" - - class CameraCapture : public px4::ScheduledWorkItem { public: @@ -98,6 +96,7 @@ class CameraCapture : public px4::ScheduledWorkItem static struct work_s _work_publisher; private: + int _capture_channel = 5; ///< by default, use FMU output 6 // Publishers uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; @@ -105,11 +104,12 @@ class CameraCapture : public px4::ScheduledWorkItem // Subscribers uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; // Trigger Buffer struct _trig_s { uint32_t chan_index; - hrt_abstime edge_time; + hrt_abstime hrt_edge_time; uint32_t edge_state; uint32_t overflow; } _trigger{}; @@ -132,6 +132,9 @@ class CameraCapture : public px4::ScheduledWorkItem hrt_abstime _last_trig_time{0}; uint32_t _capture_overflows{0}; + hrt_abstime _pps_hrt_timestamp{0}; + uint64_t _pps_rtc_timestamp{0}; + // Signal capture callback void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt index 176e223403cf..77604edfc0db 100644 --- a/src/drivers/camera_trigger/CMakeLists.txt +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -30,10 +30,18 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + px4_add_module( MODULE drivers__camera_trigger MAIN camera_trigger COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" SRCS camera_trigger.cpp interfaces/src/camera_interface.cpp diff --git a/src/drivers/camera_trigger/Kconfig b/src/drivers/camera_trigger/Kconfig new file mode 100644 index 000000000000..b07f7eec0357 --- /dev/null +++ b/src/drivers/camera_trigger/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_CAMERA_TRIGGER + bool "camera_trigger" + default n + ---help--- + Enable support for camera_trigger \ No newline at end of file diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index cae71e5aea88..8885e732f225 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -43,6 +43,7 @@ * @author Lorenz Meier */ +#include #include #include #include @@ -58,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -172,6 +174,8 @@ class CameraTrigger : public px4::ScheduledWorkItem bool _turning_on{false}; matrix::Vector2f _last_shoot_position{0.f, 0.f}; bool _valid_position{false}; + hrt_abstime _pps_hrt_timestamp{0}; + uint64_t _pps_rtc_timestamp{0}; //Camera Auto Mount Pivoting Oblique Survey (CAMPOS) uint32_t _CAMPOS_num_poses{0}; @@ -185,6 +189,7 @@ class CameraTrigger : public px4::ScheduledWorkItem uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; orb_advert_t _trigger_pub{nullptr}; @@ -196,10 +201,8 @@ class CameraTrigger : public px4::ScheduledWorkItem param_t _p_min_interval; param_t _p_distance; param_t _p_interface; - param_t _p_cam_cap_fback; trigger_mode_t _trigger_mode{TRIGGER_MODE_NONE}; - int32_t _cam_cap_fback{0}; camera_interface_mode_t _camera_interface_mode{CAMERA_INTERFACE_MODE_GPIO}; CameraInterface *_camera_interface{nullptr}; ///< instance of camera interface @@ -262,7 +265,6 @@ CameraTrigger::CameraTrigger() : _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); _p_interface = param_find("TRIG_INTERFACE"); - _p_cam_cap_fback = param_find("CAM_CAP_FBACK"); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); @@ -270,7 +272,6 @@ CameraTrigger::CameraTrigger() : param_get(_p_distance, &_distance); param_get(_p_mode, (int32_t *)&_trigger_mode); param_get(_p_interface, (int32_t *)&_camera_interface_mode); - param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback); switch (_camera_interface_mode) { #ifdef __PX4_NUTTX @@ -295,7 +296,7 @@ CameraTrigger::CameraTrigger() : break; default: - PX4_ERR("unknown camera interface mode: %i", (int)_camera_interface_mode); + PX4_ERR("unknown camera interface mode: %d", static_cast(_camera_interface_mode)); break; } @@ -313,12 +314,7 @@ CameraTrigger::CameraTrigger() : // Advertise critical publishers here, because we cannot advertise in interrupt context camera_trigger_s trigger{}; - if (!_cam_cap_fback) { - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); - - } else { - _trigger_pub = orb_advertise(ORB_ID(camera_trigger_secondary), &trigger); - } + _trigger_pub = orb_advertise_queue(ORB_ID(camera_trigger), &trigger, camera_trigger_s::ORB_QUEUE_LENGTH); } CameraTrigger::~CameraTrigger() @@ -519,8 +515,8 @@ CameraTrigger::Run() _turning_on = false; // these flags are used to detect state changes in the command loop - bool main_state = _trigger_enabled; - bool pause_state = _trigger_paused; + bool previous_trigger_state = _trigger_enabled; + bool previous_trigger_paused = _trigger_paused; bool updated = _command_sub.update(&cmd); @@ -693,8 +689,8 @@ CameraTrigger::Run() unknown_cmd: // State change handling - if ((main_state != _trigger_enabled) || - (pause_state != _trigger_paused) || + if ((previous_trigger_state != _trigger_enabled) || + (previous_trigger_paused != _trigger_paused) || _one_shot) { if (_trigger_enabled || _one_shot) { // Just got enabled via a command @@ -710,8 +706,9 @@ CameraTrigger::Run() poll_interval_usec = 3000000; _turning_on = true; } + } - } else if (!_trigger_enabled || _trigger_paused) { // Just got disabled/paused via a command + if ((!_trigger_enabled || _trigger_paused) && !_one_shot) { // Just got disabled/paused via a command // Power off the camera if we are disabled if (_camera_interface->is_powered_on() && @@ -769,7 +766,7 @@ CameraTrigger::Run() // Command ACK handling if (updated && need_ack) { - PX4_DEBUG("acknowledging command %d, result=%d", cmd.command, cmd_result); + PX4_DEBUG("acknowledging command %" PRId32 ", result=%u", cmd.command, cmd_result); vehicle_command_ack_s command_ack{}; command_ack.command = cmd.command; command_ack.result = (uint8_t)cmd_result; @@ -829,24 +826,33 @@ CameraTrigger::engage(void *arg) return; } + pps_capture_s pps_capture; + + if (trig->_pps_capture_sub.update(&pps_capture)) { + trig->_pps_hrt_timestamp = pps_capture.timestamp; + trig->_pps_rtc_timestamp = pps_capture.rtc_timestamp; + } + // Send camera trigger message. This messages indicates that we sent // the camera trigger request. Does not guarantee capture. camera_trigger_s trigger{}; - timespec tv{}; - px4_clock_gettime(CLOCK_REALTIME, &tv); - trigger.timestamp_utc = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + if (trig->_pps_hrt_timestamp > 0) { + // Current RTC time (RTC time captured by the PPS module + elapsed time since capture) + trigger.timestamp_utc = trig->_pps_rtc_timestamp + hrt_elapsed_time(&trig->_pps_hrt_timestamp); + + } else { + // No PPS capture received, use RTC clock as fallback + timespec tv{}; + px4_clock_gettime(CLOCK_REALTIME, &tv); + trigger.timestamp_utc = ts_to_abstime(&tv); + } trigger.seq = trig->_trigger_seq; trigger.feedback = false; trigger.timestamp = hrt_absolute_time(); - if (!trig->_cam_cap_fback) { - orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); - - } else { - orb_publish(ORB_ID(camera_trigger_secondary), trig->_trigger_pub, &trigger); - } + orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); // increment frame count trig->_trigger_seq++; @@ -895,9 +901,9 @@ CameraTrigger::keep_alive_down(void *arg) void CameraTrigger::status() { - PX4_INFO("main state : %s", _trigger_enabled ? "enabled" : "disabled"); - PX4_INFO("pause state : %s", _trigger_paused ? "paused" : "active"); - PX4_INFO("mode : %i", _trigger_mode); + PX4_INFO("trigger enabled : %s", _trigger_enabled ? "enabled" : "disabled"); + PX4_INFO("trigger paused : %s", _trigger_paused ? "paused" : "active"); + PX4_INFO("mode : %d", static_cast(_trigger_mode)); if (_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON || _trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) { diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 1ed5c7cd6bcb..e51bacf7baa7 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -131,15 +131,14 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); * Camera trigger pin * * Selects which FMU pin is used (range: AUX1-AUX8 on Pixhawk controllers with an I/O board, - * MAIN1-MAIN8 on controllers without an I/O board. The PWM interface takes two pins per camera, while relay + * MAIN1-MAIN8 on controllers without an I/O board). + * + * The PWM interface takes two pins per camera, while relay * triggers on every pin individually. Example: Value 56 would trigger on pins 5 and 6. * For GPIO mode Pin 6 will be triggered followed by 5. With a value of 65 pin 5 will * be triggered followed by 6. Pins may be non contiguous. I.E. 16 or 61. * In GPIO mode the delay pin to pin is < .2 uS. * - * Note: only with a value of 56 or 78 it is possible to use the lower pins for - * actuator outputs (e.g. ESC's). - * * @min 1 * @max 12345678 * @decimal 0 @@ -148,6 +147,24 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); */ PARAM_DEFINE_INT32(TRIG_PINS, 56); +/** + * Camera trigger pin extended + * + * This Bit mask selects which FMU pin is used (range: AUX9-AUX32) + * If the value is not 0 it takes precedence over TRIG_PINS. + * + * If bits above 8 are set that value is used as the selector for trigger pins. + * greater then 8. 0x00000300 Would be Pins 9,10. If the value is + * + * + * @min 0 + * @max 2147483647 + * @decimal 0 + * @reboot_required true + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_PINS_EX, 0); + /** * Camera trigger distance * diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index bc93a8cb3b17..855cdd72c813 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -33,39 +33,89 @@ #include "camera_interface.h" #include +#include void CameraInterface::get_pins() { - // Get parameter handle - _p_pin = param_find("TRIG_PINS"); - - if (_p_pin == PARAM_INVALID) { - PX4_ERR("param TRIG_PINS not found"); - return; - } - - int pin_list; - param_get(_p_pin, &pin_list); - // Set all pins as invalid for (unsigned i = 0; i < arraySize(_pins); i++) { _pins[i] = -1; } - // Convert number to individual channels - unsigned i = 0; - int single_pin; + param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); + int32_t ctrl_alloc = 0; + + if (p_ctrl_alloc != PARAM_INVALID) { + param_get(p_ctrl_alloc, &ctrl_alloc); + } + + if (ctrl_alloc == 1) { - while ((single_pin = pin_list % 10)) { + unsigned pin_index = 0; - _pins[i] = single_pin - 1; + for (unsigned i = 0; i < 16 && 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 (_pins[i] < 0) { - _pins[i] = -1; + if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) { + if (function == 2000) { // Camera_Trigger + _pins[pin_index++] = i; + } + } } - pin_list /= 10; - i++; - } + } else { + // Get parameter handle + param_t p_pin = param_find("TRIG_PINS"); + param_t p_pin_ex = param_find("TRIG_PINS_EX"); + + if (p_pin == PARAM_INVALID && p_pin_ex == PARAM_INVALID) { + PX4_ERR("param TRIG_PINS not found"); + return; + } + + int32_t pin_list = 0; + int32_t pin_list_ex = 0; + + if (p_pin_ex != PARAM_INVALID) { + param_get(p_pin_ex, &pin_list_ex); + } + if (p_pin != PARAM_INVALID) { + param_get(p_pin, &pin_list); + } + + if (pin_list_ex == 0) { + + // Convert number to individual channels + + unsigned i = 0; + int single_pin; + + while ((single_pin = pin_list % 10)) { + + _pins[i] = single_pin - 1; + + if (_pins[i] < 0) { + _pins[i] = -1; + } + + pin_list /= 10; + i++; + } + + } else { + unsigned int p = 0; + + for (unsigned int i = 0; i < arraySize(_pins); i++) { + int32_t v = (pin_list_ex & (1 << i)) ? i : -1; + + if (v > 0) { + _pins[p++] = v; + } + } + } + } } diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index 7ffe55cf0e99..f9079ea2bad2 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -97,8 +97,6 @@ class CameraInterface */ void get_pins(); - param_t _p_pin{PARAM_INVALID}; - - int _pins[6] {}; + int _pins[32] {}; }; diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index e936d392929f..0aeb91602004 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -51,15 +51,35 @@ CameraInterfaceGPIO::CameraInterfaceGPIO() setup(); } +CameraInterfaceGPIO::~CameraInterfaceGPIO() +{ + unsigned channel = 0; + + while (_allocated_channels != 0) { + if (((1u << channel) & _allocated_channels)) { + io_timer_unallocate_channel(channel); + _allocated_channels &= ~(1u << channel); + } + + ++channel; + } +} + void CameraInterfaceGPIO::setup() { + _allocated_channels = 0; + for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) { // Pin range is from 0 to num_gpios - 1 if (_pins[i] >= 0 && t < (int)arraySize(_triggers)) { uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]); - _triggers[t++] = gpio; - px4_arch_configgpio(gpio); - px4_arch_gpiowrite(gpio, false ^ _trigger_invert); + + if (io_timer_allocate_channel(_pins[i], IOTimerChanMode_Trigger) == 0) { + _allocated_channels |= 1 << _pins[i]; + _triggers[t++] = gpio; + px4_arch_configgpio(gpio); + px4_arch_gpiowrite(gpio, false ^ _trigger_invert); + } } } } @@ -80,7 +100,11 @@ void CameraInterfaceGPIO::info() PX4_INFO_RAW("GPIO trigger mode, pins enabled: "); for (unsigned i = 0; i < arraySize(_pins); ++i) { - PX4_INFO_RAW("[%d]", _pins[i]); + if (_pins[i] < 0) { + continue; + } + + PX4_INFO_RAW("[%d]", _pins[i] + 1); } PX4_INFO_RAW(", polarity : %s\n", _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h index 133a8d316a33..da81fa20e6dd 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.h +++ b/src/drivers/camera_trigger/interfaces/src/gpio.h @@ -49,14 +49,14 @@ class CameraInterfaceGPIO : public CameraInterface { public: CameraInterfaceGPIO(); - virtual ~CameraInterfaceGPIO() = default; + virtual ~CameraInterfaceGPIO(); void trigger(bool trigger_on_true); void info(); private: - static const int num_gpios = DIRECT_PWM_OUTPUT_CHANNELS > 6 ? 6 : DIRECT_PWM_OUTPUT_CHANNELS; + static const int num_gpios = 32; void setup(); @@ -65,6 +65,7 @@ class CameraInterfaceGPIO : public CameraInterface bool _trigger_invert{false}; uint32_t _triggers[num_gpios] {}; + uint32_t _allocated_channels{0}; }; #endif /* ifdef __PX4_NUTTX */ diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index ced85a061109..9b54dd7b0b44 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -68,12 +68,27 @@ void CameraInterfacePWM::setup() } // Initialize and arm channels - up_pwm_trigger_init(pin_bitmask); + int ret = up_pwm_trigger_init(pin_bitmask); + + if (ret < 0) { + PX4_ERR("up_pwm_trigger_init failed (%i)", ret); + pin_bitmask = 0; + + } else { + pin_bitmask = ret; + } + + // Clear pins that could not be initialized + for (unsigned i = 0; i < arraySize(_pins); i++) { + if (_pins[i] >= 0 && ((1 << _pins[i]) & pin_bitmask) == 0) { + _pins[i] = -1; + } + } // Set neutral pulsewidths for (unsigned i = 0; i < arraySize(_pins); i++) { if (_pins[i] >= 0) { - up_pwm_trigger_set(_pins[i], math::constrain(_pwm_camera_neutral, 0, 2000)); + up_pwm_trigger_set(_pins[i], math::constrain(_pwm_camera_neutral, (int32_t) 0, (int32_t) 2000)); } } @@ -84,15 +99,25 @@ void CameraInterfacePWM::trigger(bool trigger_on_true) for (unsigned i = 0; i < arraySize(_pins); i++) { if (_pins[i] >= 0) { // Set all valid pins to shoot or neutral levels - up_pwm_trigger_set(_pins[i], math::constrain(trigger_on_true ? _pwm_camera_shoot : _pwm_camera_neutral, 0, 2000)); + up_pwm_trigger_set(_pins[i], math::constrain(trigger_on_true ? _pwm_camera_shoot : _pwm_camera_neutral, (int32_t) 0, + (int32_t) 2000)); } } } void CameraInterfacePWM::info() { - PX4_INFO("PWM trigger mode (generic), pins enabled : [%d][%d][%d][%d][%d][%d]", - _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]); + PX4_INFO_RAW("PWM trigger mode, pins enabled: "); + + for (unsigned i = 0; i < arraySize(_pins); ++i) { + if (_pins[i] < 0) { + continue; + } + + PX4_INFO_RAW("[%d]", _pins[i] + 1); + } + + PX4_INFO_RAW("\n"); } #endif /* ifdef __PX4_NUTTX */ diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp index ae4cd18ffa28..0f8b5f534ce6 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp @@ -69,7 +69,12 @@ void CameraInterfaceSeagull::setup() // Initialize the interface uint32_t pin_bitmask = (1 << _pins[i + 1]) | (1 << _pins[i]); - up_pwm_trigger_init(pin_bitmask); + int ret = up_pwm_trigger_init(pin_bitmask); + + if (ret != (int)pin_bitmask) { + PX4_WARN("up_pwm_trigger_init failed (%i)", ret); + continue; + } // Set both interface pins to disarmed int ret1 = up_pwm_trigger_set(_pins[i + 1], PWM_CAMERA_DISARMED); diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp new file mode 100644 index 000000000000..c0c7a32cc9fe --- /dev/null +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 EscClient.hpp + * + * Client-side implementation of UDRAL specification ESC service + * + * Publishes the following Cyphal messages: + * reg.drone.service.actuator.common.sp.Value8.0.1 + * reg.drone.service.common.Readiness.0.1 + * + * Subscribes to the following Cyphal messages: + * reg.drone.service.actuator.common.Feedback.0.1 + * reg.drone.service.actuator.common.Status.0.1 + * + * @author Pavel Kirienko + * @author Jacob Crabill + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +// UDRAL Specification Messages +#include +#include + +/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status +class UavcanEscController : public UavcanPublisher +{ +public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; + + UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral", "esc") { }; + + ~UavcanEscController() {}; + + void update() override + { + if (_armed_sub.updated()) { + actuator_armed_s new_arming; + _armed_sub.update(&new_arming); + + if (new_arming.armed != _armed.armed) { + _armed = new_arming; + size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + + // Only publish if we have a valid publication ID set + if (_port_id == 0) { + return; + } + + reg_udral_service_common_Readiness_0_1 msg_arming {}; + + if (_armed.armed) { + msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED; + + } else if (_armed.prearmed) { + msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY; + + } else { + msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP; + } + + uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = arming_pid, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _arming_transfer_id, + }; + + int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &arming_payload_buffer + ); + } + } + } + }; + + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) + { + if (_port_id > 0) { + reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; + size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + + for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { + if (i < num_outputs) { + msg_sp.value[i] = static_cast(outputs[i]); + + } else { + // "unset" values published as NaN + msg_sp.value[i] = NAN; + } + } + + + PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1], + (double)msg_sp.value[2], (double)msg_sp.value[3]); + + uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _transfer_id, + }; + + int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &esc_sp_payload_buffer); + } + } + }; + + /** + * Sets the number of rotors + */ + void set_rotor_count(uint8_t count) { _rotor_count = count; } + +private: + /** + * ESC status message reception will be reported via this callback. + */ + void esc_status_sub_cb(const CanardRxTransfer &msg); + + uint8_t _rotor_count {0}; + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + actuator_armed_s _armed {}; + + CanardTransferID _arming_transfer_id; +}; diff --git a/src/drivers/uavcan_v1/Actuators/EscServer.hpp b/src/drivers/cyphal/Actuators/EscServer.hpp similarity index 93% rename from src/drivers/uavcan_v1/Actuators/EscServer.hpp rename to src/drivers/cyphal/Actuators/EscServer.hpp index af2ba1338c50..962e21be0d34 100644 --- a/src/drivers/uavcan_v1/Actuators/EscServer.hpp +++ b/src/drivers/cyphal/Actuators/EscServer.hpp @@ -34,14 +34,14 @@ /** * @file EscServer.hpp * - * Server-side implementation of DS-15 specification ESC service + * Server-side implementation of UDRAL specification ESC service * (Used for CANNode control of an ESC via PWM output) * - * Subscribes to the following UAVCAN v1 messages: + * Subscribes to the following Cyphal messages: * reg.drone.service.actuator.common.sp.Value8.0.1 * reg.drone.service.common.Readiness.0.1 * - * Publishes to the following UAVCAN v1 messages: + * Publishes to the following Cyphal messages: * reg.drone.service.actuator.common.Feedback.0.1 * reg.drone.service.actuator.common.Status.0.1 * @@ -62,7 +62,7 @@ #include #include -// DS-15 Specification Messages +// UDRAL Specification Messages #include #include diff --git a/src/drivers/cyphal/CMakeLists.txt b/src/drivers/cyphal/CMakeLists.txt new file mode 100644 index 000000000000..dc3c82b7bd2b --- /dev/null +++ b/src/drivers/cyphal/CMakeLists.txt @@ -0,0 +1,145 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard) +set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types) +set(LEGACY_DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/legacy_data_types) + +px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR}) +px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) +px4_add_git_submodule(TARGET git_legacy_data_types PATH ${LEGACY_DSDL_DIR}) + +find_program(NNVG_PATH nnvg) +if(NNVG_PATH) + message("Generating Cyphal DSDL headers using Nunavut") + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${LEGACY_DSDL_DIR}/uavcan ${LEGACY_DSDL_DIR}/legacy) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan) +else() + message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") +endif() + +set(SRCS) +set(DPNDS) + +if(${PX4_PLATFORM} MATCHES "nuttx") + if(CONFIG_NET_CAN) + list(APPEND SRCS + CanardSocketCAN.cpp + CanardSocketCAN.hpp + ) + elseif(CONFIG_CAN_EXTID) + list(APPEND SRCS + CanardNuttXCDev.cpp + CanardNuttXCDev.hpp + ) + endif() +endif() + +if(CONFIG_CYPHAL_NODE_MANAGER) + list(APPEND SRCS + NodeManager.hpp + NodeManager.cpp + ) +endif() + +if(CONFIG_CYPHAL_NODE_CLIENT) + list(APPEND SRCS + NodeClient.hpp + NodeClient.cpp + ) + list(APPEND DPNDS + drivers_bootloaders + ) +endif() + +# Temporary measure to get Kconfig option as defines into this application +# Ideally we want nicely decouple and define this in kconfig.cmake +# But then we need to overhaul the src modules naming convention +set(DRIVERS_CYPHAL_OPTIONS) +get_cmake_property(_variableNames VARIABLES) +foreach (_variableName ${_variableNames}) + string(REGEX MATCH "^CONFIG_CYPHAL_" CYPHAL_OPTION ${_variableName}) + + if(CYPHAL_OPTION) + if(${${_variableName}} STREQUAL "y") + list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=1") + else() + list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=${${_variableName}}") + endif() + endif() +endforeach() + +remove_definitions(-Werror) + +px4_add_module( + MODULE drivers__cyphal + MAIN cyphal + STACK_MAIN 4096 + COMPILE_FLAGS + #-DCANARD_ASSERT + -DUINT32_C\(x\)=__UINT32_C\(x\) + -O0 + ${DRIVERS_CYPHAL_OPTIONS} + INCLUDES + ${LIBCANARD_DIR}/libcanard/ + ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated + SRCS + PublicationManager.cpp + PublicationManager.hpp + SubscriptionManager.cpp + SubscriptionManager.hpp + ParamManager.hpp + ParamManager.cpp + Cyphal.cpp + Cyphal.hpp + CanardHandle.cpp + Publishers/uORB/uorb_publisher.cpp + Subscribers/uORB/uorb_subscriber.cpp + ${SRCS} + o1heap/o1heap.c + o1heap/o1heap.h + ${LIBCANARD_DIR}/libcanard/canard.c + ${LIBCANARD_DIR}/libcanard/canard.h + MODULE_CONFIG + module.yaml + DEPENDS + git_libcanard + git_public_regulated_data_types + git_legacy_data_types + version + ${DPNDS} + ) + +# libcanard 3.0 introduces this warning, for now no intention to fix it thus we ignore this warning +set_source_files_properties(${LIBCANARD_DIR}/libcanard/canard.c PROPERTIES COMPILE_FLAGS -Wno-cast-align) diff --git a/src/drivers/cyphal/CanardHandle.cpp b/src/drivers/cyphal/CanardHandle.cpp new file mode 100644 index 000000000000..a197712afeac --- /dev/null +++ b/src/drivers/cyphal/CanardHandle.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * 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 "CanardHandle.hpp" + +#include +#include +#include + +#include + +#include "o1heap/o1heap.h" + +#include "Subscribers/BaseSubscriber.hpp" + +#if defined(__PX4_NUTTX) +# if defined(CONFIG_NET_CAN) +# include "CanardSocketCAN.hpp" +# elif defined(CONFIG_CAN) +# include "CanardNuttXCDev.hpp" +# endif // CONFIG_CAN +#endif // NuttX + + +O1HeapInstance *cyphal_allocator{nullptr}; + +static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(cyphal_allocator, amount); } +static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(cyphal_allocator, pointer); } + + +CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes) +{ + _cyphal_heap = memalign(O1HEAP_ALIGNMENT, HeapSize); + cyphal_allocator = o1heapInit(_cyphal_heap, HeapSize, nullptr, nullptr); + + if (cyphal_allocator == nullptr) { + PX4_ERR("o1heapInit failed with size %u", HeapSize); + } + + _canard_instance = canardInit(&memAllocate, &memFree); + + _canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point. + + _queue = canardTxInit(capacity, mtu_bytes); + +#if defined(__PX4_NUTTX) +# if defined(CONFIG_NET_CAN) + _can_interface = new CanardSocketCAN(); +# elif defined(CONFIG_CAN) + _can_interface = new CanardNuttXCDev(); +# endif // CONFIG_CAN +#endif // NuttX + +} + +CanardHandle::~CanardHandle() +{ + _can_interface->close(); + delete _can_interface; + _can_interface = nullptr; + + delete static_cast(_cyphal_heap); + _cyphal_heap = nullptr; + +} + + +bool CanardHandle::init() +{ + if (_can_interface) { + if (_can_interface->init() == PX4_OK) { + return true; + } + } + + return false; +} + +void CanardHandle::receive() +{ + /* Process received messages */ + + uint8_t data[64] {}; + CanardRxFrame received_frame{}; + received_frame.frame.payload = &data; + + while (_can_interface->receive(&received_frame) > 0) { + CanardRxTransfer receive{}; + CanardRxSubscription *subscription = nullptr; + int32_t result = canardRxAccept(&_canard_instance, received_frame.timestamp_usec, &received_frame.frame, 0, &receive, + &subscription); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if + // the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + // Reception of an invalid frame is NOT an error. + PX4_ERR("Receive error %" PRId32" \n", result); + + } else if (result == 1) { + // A transfer has been received, process it. + // PX4_INFO("received Port ID: %d", receive.port_id); + + if (subscription != nullptr) { + UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference; + sub_instance->callback(receive); + + } else { + PX4_ERR("No matching sub for %d", receive.metadata.port_id); + } + + // Deallocate the dynamic memory afterwards. + _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); + + } else { + //PX4_INFO("RX canard %d", result); + } + } + +} + +void CanardHandle::transmit() +{ + // Look at the top of the TX queue. + for (const CanardTxQueueItem *ti = NULL; (ti = canardTxPeek(&_queue)) != NULL;) { // Peek at the top of the queue. + if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > hrt_absolute_time())) { // Check the deadline. + // Send the frame. Redundant interfaces may be used here. + const int tx_res = _can_interface->transmit(*ti); + + if (tx_res < 0) { + PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno)); + + } else if (tx_res == 0) { + // Timeout - just exit and try again later + break; + } + + } + + // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate: + _canard_instance.memory_free(&_canard_instance, canardTxPop(&_queue, ti)); + } +} + +int32_t CanardHandle::TxPush(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *const metadata, + const size_t payload_size, + const void *const payload) +{ + return canardTxPush(&_queue, &_canard_instance, tx_deadline_usec, metadata, payload_size, payload); +} + +int8_t CanardHandle::RxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription *const out_subscription) +{ + return canardRxSubscribe(&_canard_instance, transfer_kind, port_id, extent, transfer_id_timeout_usec, out_subscription); +} + +int8_t CanardHandle::RxUnsubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id) +{ + return canardRxUnsubscribe(&_canard_instance, transfer_kind, port_id); +} + +CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind) +{ + return _canard_instance.rx_subscriptions[kind]; +} + +O1HeapDiagnostics CanardHandle::getO1HeapDiagnostics() +{ + return o1heapGetDiagnostics(cyphal_allocator); +} + +int32_t CanardHandle::mtu() +{ + return _queue.mtu_bytes; +} + +CanardNodeID CanardHandle::node_id() +{ + return _canard_instance.node_id; +} + +void CanardHandle::set_node_id(CanardNodeID id) +{ + _canard_instance.node_id = id; +} diff --git a/src/drivers/cyphal/CanardHandle.hpp b/src/drivers/cyphal/CanardHandle.hpp new file mode 100644 index 000000000000..1e6c787c311e --- /dev/null +++ b/src/drivers/cyphal/CanardHandle.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include "o1heap/o1heap.h" +#include "CanardInterface.hpp" + +class CanardHandle +{ + /* + * This memory is allocated for the 01Heap allocator used by + * libcanard to store incoming/outcoming data + * Current size of 8192 bytes is arbitrary, should be optimized further + * when more nodes and messages are on the CAN bus + */ + static constexpr unsigned HeapSize = 8192; + +public: + CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes); + ~CanardHandle(); + + bool init(); + + void receive(); + void transmit(); + + int32_t TxPush(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *const metadata, + const size_t payload_size, + const void *const payload); + + int8_t RxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription *const out_subscription); + int8_t RxUnsubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id); + CanardTreeNode *getRxSubscriptions(CanardTransferKind kind); + O1HeapDiagnostics getO1HeapDiagnostics(); + + int32_t mtu(); + CanardNodeID node_id(); + void set_node_id(CanardNodeID id); + +private: + CanardInterface *_can_interface; + + CanardInstance _canard_instance; + + CanardTxQueue _queue; + + void *_cyphal_heap{nullptr}; + +}; diff --git a/src/drivers/uavcan_v1/CanardInterface.hpp b/src/drivers/cyphal/CanardInterface.hpp similarity index 87% rename from src/drivers/uavcan_v1/CanardInterface.hpp rename to src/drivers/cyphal/CanardInterface.hpp index 088600a18a89..4e8aa4b8aa0b 100644 --- a/src/drivers/uavcan_v1/CanardInterface.hpp +++ b/src/drivers/cyphal/CanardInterface.hpp @@ -35,6 +35,14 @@ #include +/// One frame stored in the transmission queue along with its metadata. +struct CanardRxFrame { + CanardMicrosecond timestamp_usec; + + /// The actual CAN frame data. + CanardFrame frame; +}; + class CanardInterface { public: @@ -48,12 +56,12 @@ class CanardInterface /// Send a CanardFrame /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - virtual int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0) = 0; + virtual int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0) = 0; /// Receive a CanardFrame /// This function is blocking /// The return value is number of bytes received, negative value on error. - virtual int16_t receive(CanardFrame *rxf) = 0; + virtual int16_t receive(CanardRxFrame *rxf) = 0; private: diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp b/src/drivers/cyphal/CanardNuttXCDev.cpp similarity index 86% rename from src/drivers/uavcan_v1/CanardNuttXCDev.cpp rename to src/drivers/cyphal/CanardNuttXCDev.cpp index f854cbf60e3f..bbb4dbaec847 100644 --- a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp +++ b/src/drivers/cyphal/CanardNuttXCDev.cpp @@ -65,7 +65,7 @@ int CanardNuttXCDev::init() return 0; } -int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) +int16_t CanardNuttXCDev::transmit(const CanardTxQueueItem &txf, int timeout_ms) { if (_fd < 0) { return -1; @@ -93,13 +93,13 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) struct can_msg_s transmit_msg {}; - transmit_msg.cm_hdr.ch_id = txf.extended_can_id; + transmit_msg.cm_hdr.ch_id = txf.frame.extended_can_id; - transmit_msg.cm_hdr.ch_dlc = txf.payload_size; + transmit_msg.cm_hdr.ch_dlc = txf.frame.payload_size; transmit_msg.cm_hdr.ch_extid = 1; - memcpy(transmit_msg.cm_data, txf.payload, txf.payload_size); + memcpy(transmit_msg.cm_data, txf.frame.payload, txf.frame.payload_size); const size_t msg_len = CAN_MSGLEN(transmit_msg.cm_hdr.ch_dlc); @@ -112,7 +112,7 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) return 1; } -int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) +int16_t CanardNuttXCDev::receive(CanardRxFrame *received_frame) { if ((_fd < 0) || (received_frame == nullptr)) { return -1; @@ -126,7 +126,7 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) // Any recieved CAN messages will cause the poll statement to unblock and run // This way CAN read runs with minimal latency. // Note that multiple messages may be received in a short time, so this will try to read any availible in a loop - ::poll(&fds, 1, 10); + ::poll(&fds, 1, 0); // Only execute this part if can0 is changed. if (fds.revents & POLLIN) { @@ -140,9 +140,9 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) return -1; } else { - received_frame->extended_can_id = receive_msg.cm_hdr.ch_id; - received_frame->payload_size = receive_msg.cm_hdr.ch_dlc; - memcpy((void *)received_frame->payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc); + received_frame->frame.extended_can_id = receive_msg.cm_hdr.ch_id; + received_frame->frame.payload_size = receive_msg.cm_hdr.ch_dlc; + memcpy((void *)received_frame->frame.payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc); return nbytes; } } diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.hpp b/src/drivers/cyphal/CanardNuttXCDev.hpp similarity index 91% rename from src/drivers/uavcan_v1/CanardNuttXCDev.hpp rename to src/drivers/cyphal/CanardNuttXCDev.hpp index 6f0b79a4e5f1..e9d345afd092 100644 --- a/src/drivers/uavcan_v1/CanardNuttXCDev.hpp +++ b/src/drivers/cyphal/CanardNuttXCDev.hpp @@ -51,15 +51,15 @@ class CanardNuttXCDev : public CanardInterface /// The return value is 0 on succes and -1 on error int init(); - /// Send a CanardFrame to the CanardSocketInstance socket + /// Send a CanardTxQueueItem to the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0); + int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0); - /// Receive a CanardFrame from the CanardSocketInstance socket + /// Receive a CanardRxFrame from the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes received, negative value on error. - int16_t receive(CanardFrame *rxf); + int16_t receive(CanardRxFrame *rxf); private: int _fd{-1}; diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/cyphal/CanardSocketCAN.cpp similarity index 76% rename from src/drivers/uavcan_v1/CanardSocketCAN.cpp rename to src/drivers/cyphal/CanardSocketCAN.cpp index c26ad2a9ced9..ef1c3afc8b73 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/cyphal/CanardSocketCAN.cpp @@ -39,6 +39,15 @@ #include +uint64_t getMonotonicTimestampUSec(void) +{ + struct timespec ts {}; + + clock_gettime(CLOCK_MONOTONIC, &ts); + + return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL; +} + int CanardSocketCAN::init() { const char *const can_iface_name = "can0"; @@ -120,7 +129,7 @@ int CanardSocketCAN::init() _send_cmsg->cmsg_level = SOL_CAN_RAW; _send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE; _send_cmsg->cmsg_len = sizeof(struct timeval); - _send_tv = (struct timeval *)CMSG_DATA(&_send_cmsg); + _send_tv = (struct timeval *)CMSG_DATA(_send_cmsg); // Setup RX msg _recv_iov.iov_base = &_recv_frame; @@ -143,31 +152,34 @@ int CanardSocketCAN::init() return 0; } -int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms) +int16_t CanardSocketCAN::transmit(const CanardTxQueueItem &txf, int timeout_ms) { /* Copy CanardFrame to can_frame/canfd_frame */ if (_can_fd) { - _send_frame.can_id = txf.extended_can_id | CAN_EFF_FLAG; - _send_frame.len = txf.payload_size; - memcpy(&_send_frame.data, txf.payload, txf.payload_size); + _send_frame.can_id = txf.frame.extended_can_id | CAN_EFF_FLAG; + _send_frame.len = txf.frame.payload_size; + memcpy(&_send_frame.data, txf.frame.payload, txf.frame.payload_size); } else { struct can_frame *frame = (struct can_frame *)&_send_frame; - frame->can_id = txf.extended_can_id | CAN_EFF_FLAG; - frame->can_dlc = txf.payload_size; - memcpy(&frame->data, txf.payload, txf.payload_size); + frame->can_id = txf.frame.extended_can_id | CAN_EFF_FLAG; + frame->can_dlc = txf.frame.payload_size; + memcpy(&frame->data, txf.frame.payload, txf.frame.payload_size); } + uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.tx_deadline_usec - hrt_absolute_time()) + + CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick + /* Set CAN_RAW_TX_DEADLINE timestamp */ - _send_tv->tv_usec = txf.timestamp_usec % 1000000ULL; - _send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL; + _send_tv->tv_usec = deadline_systick % 1000000ULL; + _send_tv->tv_sec = (deadline_systick - _send_tv->tv_usec) / 1000000ULL; - return sendmsg(_fd, &_send_msg, 1000); + return sendmsg(_fd, &_send_msg, 0); } -int16_t CanardSocketCAN::receive(CanardFrame *rxf) +int16_t CanardSocketCAN::receive(CanardRxFrame *rxf) { - int32_t result = recvmsg(_fd, &_recv_msg, 1000); + int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); if (result < 0) { return result; @@ -177,15 +189,15 @@ int16_t CanardSocketCAN::receive(CanardFrame *rxf) if (_can_fd) { struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame; - rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; - rxf->payload_size = recv_frame->len; - rxf->payload = &recv_frame->data; + rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->frame.payload_size = recv_frame->len; + rxf->frame.payload = &recv_frame->data; } else { struct can_frame *recv_frame = (struct can_frame *)&_recv_frame; - rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; - rxf->payload_size = recv_frame->can_dlc; - rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference + rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->frame.payload_size = recv_frame->can_dlc; + rxf->frame.payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference } /* Read SO_TIMESTAMP value */ diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.hpp b/src/drivers/cyphal/CanardSocketCAN.hpp similarity index 94% rename from src/drivers/uavcan_v1/CanardSocketCAN.hpp rename to src/drivers/cyphal/CanardSocketCAN.hpp index 58f2e1900158..ba93b48dbe32 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.hpp +++ b/src/drivers/cyphal/CanardSocketCAN.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -61,15 +62,21 @@ class CanardSocketCAN : public CanardInterface /// The return value is 0 on succes and -1 on error int init(); + /// Close socket connection + int close() + { + return ::close(_fd); + } + /// Send a CanardFrame to the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0); + int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0); /// Receive a CanardFrame from the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes received, negative value on error. - int16_t receive(CanardFrame *rxf); + int16_t receive(CanardRxFrame *rxf); // TODO implement ioctl for CAN filter //int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters); diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp new file mode 100644 index 000000000000..dd6fd8b71108 --- /dev/null +++ b/src/drivers/cyphal/Cyphal.cpp @@ -0,0 +1,408 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "Cyphal.hpp" + +#include +#include + + +#ifdef CONFIG_CYPHAL_APP_DESCRIPTOR +#include "boot_app_shared.h" +/* + * This is the AppImageDescriptor 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 of this application +*/ +boot_app_shared_section app_descriptor_t AppDescriptor = { + .signature = APP_DESCRIPTOR_SIGNATURE, + .image_crc = 0, + .image_size = 0, + .git_hash = 0, + .major_version = APP_VERSION_MAJOR, + .minor_version = APP_VERSION_MINOR, + .board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR, + .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff } +}; +#endif + +using namespace time_literals; + +CyphalNode *CyphalNode::_instance; + +CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), + _canard_handle(node_id, capacity, mtu_bytes) +{ + pthread_mutex_init(&_node_mutex, nullptr); + + +#ifdef CONFIG_CYPHAL_NODE_MANAGER + _node_manager.subscribe(); +#endif + +#ifdef CONFIG_CYPHAL_NODE_CLIENT + _node_client = new NodeClient(_canard_handle, _param_manager); + + _node_client->subscribe(); +#endif + + _pub_manager.updateParams(); + + _sub_manager.subscribe(); +} + +CyphalNode::~CyphalNode() +{ + if (_instance) { + /* tell the task we want it to go away */ + _task_should_exit.store(true); + ScheduleNow(); + + unsigned i = 1000; + + do { + /* Wait for it to exit or timeout */ + usleep(5000); + + if (--i == 0) { + PX4_ERR("Failed to Stop Task - reboot needed"); + break; + } + + } while (_instance); + } + + perf_free(_cycle_perf); + perf_free(_interval_perf); +} + +int CyphalNode::start(uint32_t node_id, uint32_t bitrate) +{ + if (_instance != nullptr) { + PX4_WARN("Already started"); + return -1; + } + + bool can_fd = false; + + if (can_fd) { + _instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD); + + } else { + _instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC); + } + + if (_instance == nullptr) { + PX4_ERR("Out of memory"); + return -1; + } + + // Keep the bit rate for reboots on BenginFirmware updates + _instance->active_bitrate = bitrate; + + _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); + + return PX4_OK; +} + +void CyphalNode::init() +{ + // interface init + if (_canard_handle.init()) { + _initialized = true; + } + +} + +void CyphalNode::Run() +{ + pthread_mutex_lock(&_node_mutex); + + if (_instance != nullptr && !_initialized) { + init(); + + // return early if still not initialized + if (!_initialized) { + pthread_mutex_unlock(&_node_mutex); + return; + } + } + + // 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(); + + // Update dynamic pub/sub objects based on Port ID params + _pub_manager.updateParams(); + _sub_manager.updateParams(); + + _mixing_output.updateParams(); + } + + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) { + // send uavcan::node::Heartbeat_1_0 @ 1 Hz + sendHeartbeat(); + + // Check all publishers + _pub_manager.update(); + +#ifdef CONFIG_CYPHAL_NODE_MANAGER + _node_manager.update(); +#endif + } + +#ifdef CONFIG_CYPHAL_NODE_CLIENT + + else if (_node_client != nullptr) { + if (_canard_handle.node_id() == CANARD_NODE_ID_UNSET) { + _node_client->update(); + + } else { + delete _node_client; + } + } + +#endif + + _canard_handle.transmit(); + + _canard_handle.receive(); + + // Pop canardTx queue to send out responses to requests + _canard_handle.transmit(); + + perf_end(_cycle_perf); + + if (_instance && _task_should_exit.load()) { + ScheduleClear(); + + if (_initialized) { + _initialized = false; + } + + _instance = nullptr; + } + + pthread_mutex_unlock(&_node_mutex); +} + +template +static void traverseTree(const CanardTreeNode *const root, const F &op) // NOLINT this recursion is tightly bounded +{ + if (root != nullptr) { + traverseTree(root->lr[0], op); + op(static_cast(static_cast(root))); + traverseTree(root->lr[1], op); + } +} + +void CyphalNode::print_info() +{ + pthread_mutex_lock(&_node_mutex); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + + O1HeapDiagnostics heap_diagnostics = _canard_handle.getO1HeapDiagnostics(); + + PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64, + heap_diagnostics.allocated, heap_diagnostics.capacity, + heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size, + heap_diagnostics.oom_count); + + _pub_manager.printInfo(); + + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindMessage), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Message port id %d", sub->port_id); + + } else { + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); + } + }); + + + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindRequest), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Service response port id %d", sub->port_id); + + } else { + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); + } + }); + + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindResponse), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Service request port id %d", sub->port_id); + + } else { + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); + } + }); + + _mixing_output.printInfo(); + + pthread_mutex_unlock(&_node_mutex); +} + +static void print_usage() +{ + PX4_INFO("usage: \n" + "\tuavcannode {start|status|stop}"); +} + +extern "C" __EXPORT int cyphal_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + if (CyphalNode::instance()) { + PX4_ERR("already started"); + return 1; + } + + // CAN bitrate + int32_t bitrate = 0; + param_get(param_find("CYPHAL_BAUD"), &bitrate); + + // Node ID + int32_t node_id = 0; + param_get(param_find("CYPHAL_ID"), &node_id); + + if (node_id == -1) { + node_id = CANARD_NODE_ID_UNSET; + } + + // Start + PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate); + return CyphalNode::start(node_id, bitrate); + } + + /* commands below require the app to be started */ + CyphalNode *const inst = CyphalNode::instance(); + + if (!inst) { + PX4_ERR("application not running"); + return 1; + } + + if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) { + inst->print_info(); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + delete inst; + return 0; + } + + print_usage(); + return 1; +} + +void CyphalNode::sendHeartbeat() +{ + if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { + + uavcan_node_Heartbeat_1_0 heartbeat{}; + heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime + heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; + heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; + const hrt_abstime now = hrt_absolute_time(); + size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _uavcan_node_heartbeat_transfer_id++ + }; + + uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &payload_size); + + int32_t result = _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &_uavcan_node_heartbeat_buffer + ); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + PX4_ERR("Heartbeat transmit error %" PRId32 "", result); + } + + _uavcan_node_heartbeat_last = now; + } +} + +bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + // Note: This gets called from MixingOutput from within its update() function + // Hence, the mutex lock in UavcanMixingInterface::Run() is in effect + + /// TODO: Need esc/servo metadata / bitmask(s) + _esc_controller.update_outputs(stop_motors, outputs, num_outputs); + // _servo_controller.update_outputs(stop_motors, outputs, num_outputs); + + return true; +} + +void UavcanMixingInterface::Run() +{ + pthread_mutex_lock(&_node_mutex); + _mixing_output.update(); + _mixing_output.updateSubscriptions(); + pthread_mutex_unlock(&_node_mutex); +} diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp new file mode 100644 index 000000000000..203f11b601e9 --- /dev/null +++ b/src/drivers/cyphal/Cyphal.hpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "CanardInterface.hpp" + +#include "Publishers/Publisher.hpp" +#include "Publishers/uORB/uorb_publisher.hpp" + +#ifdef CONFIG_CYPHAL_NODE_MANAGER +#include "NodeManager.hpp" +#endif + +#ifdef CONFIG_CYPHAL_NODE_CLIENT +#include "NodeClient.hpp" +#endif + +#include "PublicationManager.hpp" +#include "SubscriptionManager.hpp" + +#include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service + +/** + * UAVCAN mixing class. + * It is separate from CyphalNode to have 2 WorkItems and therefore allowing independent scheduling + * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas CyphalNode runs at + * a fixed rate or upon bus updates). + * Both work items are expected to run on the same work queue. + */ +class UavcanMixingInterface : public OutputModuleInterface +{ +public: + UavcanMixingInterface(pthread_mutex_t &node_mutex, + UavcanEscController &esc_controller) //, UavcanServoController &servo_controller) + : OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan), + _node_mutex(node_mutex), + _esc_controller(esc_controller)/*, + _servo_controller(servo_controller)*/ {} + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + void mixerChanged() override {}; + + void printInfo() { _mixing_output.printStatus(); } + + MixingOutput &mixingOutput() { return _mixing_output; } + + /// For use with PR-16808 once merged + // const char *get_param_prefix() override { return "UCAN1_ACT"; } + +protected: + void Run() override; +private: + friend class CyphalNode; + pthread_mutex_t &_node_mutex; + UavcanEscController &_esc_controller; + // UavcanServoController &_servo_controller; + MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; +}; + +class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem +{ + + /* + * Base interval, has to be complemented with events from the CAN driver + * and uORB topics sending data, to decrease response time. + */ + static constexpr unsigned ScheduleIntervalMs = 10; + +public: + + CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes); + ~CyphalNode() override; + + static int start(uint32_t node_id, uint32_t bitrate); + + void print_info(); + + static CyphalNode *instance() { return _instance; } + + /* The bit rate that can be passed back to the bootloader */ + int32_t active_bitrate{0}; + +private: + void init(); + void Run() override; + void fill_node_info(); + + // Sends a heartbeat at 1s intervals + void sendHeartbeat(); + + px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver + + bool _initialized{false}; ///< number of actuators currently available + + static CyphalNode *_instance; + + CanardHandle _canard_handle; + + pthread_mutex_t _node_mutex; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + + // uavcan::node::Heartbeat_1_0 + uint8_t _uavcan_node_heartbeat_buffer[uavcan_node_Heartbeat_1_0_EXTENT_BYTES_]; + hrt_abstime _uavcan_node_heartbeat_last{0}; + CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_uavcan_v1_enable, + (ParamInt) _param_uavcan_v1_id, + (ParamInt) _param_uavcan_v1_baud + ) + + UavcanParamManager _param_manager; + +#ifdef CONFIG_CYPHAL_NODE_MANAGER + NodeManager _node_manager {_canard_handle, _param_manager}; +#endif + +#ifdef CONFIG_CYPHAL_NODE_CLIENT + NodeClient *_node_client {nullptr}; +#endif + + PublicationManager _pub_manager {_canard_handle, _param_manager}; + SubscriptionManager _sub_manager {_canard_handle, _param_manager}; + + /// TODO: Integrate with PublicationManager + UavcanEscController _esc_controller {_canard_handle, _param_manager}; + + UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; + +}; diff --git a/src/drivers/cyphal/Kconfig b/src/drivers/cyphal/Kconfig new file mode 100644 index 000000000000..050cd891f895 --- /dev/null +++ b/src/drivers/cyphal/Kconfig @@ -0,0 +1,110 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# +menuconfig DRIVERS_CYPHAL + bool "Cyphal" + default n + ---help--- + Enable support for Cyphal + +if DRIVERS_CYPHAL + choice + prompt "Cyphal Mode" + + config CYPHAL_FMU + bool "Server (FMU)" + + config CYPHAL_CLIENT + bool "Client (Peripheral)" + + endchoice + + config CYPHAL_NODE_MANAGER + bool "Node manager" + default y + depends on CYPHAL_FMU + help + Implement Cyphal PNP server functionality and manages discovered nodes + + config CYPHAL_NODE_CLIENT + bool "Node client" + default y + depends on CYPHAL_CLIENT + help + Implement Cyphal PNP client functionality + + config CYPHAL_APP_DESCRIPTOR + bool "UAVCAN v0 bootloader app descriptor" + default n + depends on CYPHAL_CLIENT && DRIVERS_BOOTLOADERS + help + When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined + + + menu "Publisher support" + + config CYPHAL_GNSS_PUBLISHER + bool "GNSS Publisher" + default n + + config CYPHAL_ESC_CONTROLLER + bool "ESC Controller" + default n + + config CYPHAL_READINESS_PUBLISHER + bool "Readiness Publisher" + default n + + config CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + bool "uORB actuator_outputs publisher" + default n + + config CYPHAL_UORB_SENSOR_GPS_PUBLISHER + bool "uORB sensor_gps publisher" + default n + + endmenu + + menu "Subscriber support" + + config CYPHAL_ESC_SUBSCRIBER + bool "ESC Subscriber" + default n + + config CYPHAL_GNSS_SUBSCRIBER_0 + bool "GNSS Subscriber 0" + default n + + config CYPHAL_GNSS_SUBSCRIBER_1 + bool "GNSS Subscriber 1" + default n + + config CYPHAL_BMS_SUBSCRIBER + bool "BMS Subscriber" + default n + + config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + bool "uORB sensor_gps Subscriber" + default n + endmenu + + menu "Advertised Services" + config CYPHAL_GETINFO_RESPONDER + bool "GetInfo1.0 responder" + default y + help + Responds to uavcan.node.GetInfo.1.0 request sending over node information + See https://github.com/UAVCAN/public_regulated_data_types/blob/master/uavcan/node/430.GetInfo.1.0.uavcan for full response + + config CYPHAL_EXECUTECOMMAND_RESPONDER + bool "ExecuteCommand1.0 responder" + default n + help + To be implemented + endmenu + + menu "Service invokers" + endmenu + +endif #DRIVERS_CYPHAL diff --git a/src/drivers/cyphal/NodeClient.cpp b/src/drivers/cyphal/NodeClient.cpp new file mode 100644 index 000000000000..0f2ae4107604 --- /dev/null +++ b/src/drivers/cyphal/NodeClient.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * 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 NodeClient.cpp + * + * Defines basic implementation of client UAVCAN PNP for requesting a Node ID + * + * @author Peter van der Perk + */ + +#define PNP_UNIQUE_ID_SIZE 16 // 128 bit unique id + +#include +#include "NodeClient.hpp" + +void NodeClient::callback(const CanardRxTransfer &receive) +{ + if (receive.metadata.remote_node_id != CANARD_NODE_ID_UNSET && _canard_handle.node_id() == CANARD_NODE_ID_UNSET) { + + int32_t allocated = CANARD_NODE_ID_UNSET; + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { + uavcan_pnp_NodeIDAllocationData_2_0 msg; + + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + + if (memcmp(msg.unique_id, px4_guid, sizeof(msg.unique_id)) == 0) { + allocated = msg.node_id.value; + } + + } else { + uavcan_pnp_NodeIDAllocationData_1_0 msg; + + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + + if (msg.allocated_node_id.count > 0) { + if (msg.unique_id_hash == (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF)) { + allocated = msg.allocated_node_id.elements[0].value; + } + } + } + + if (allocated == CANARD_NODE_ID_UNSET) { + return; // UID mismatch. + } + + if (allocated <= 0 || allocated >= (int32_t)CANARD_NODE_ID_MAX) + // Allocated node-ID ignored because it exceeds max_node_id + { + return; + } + + _canard_handle.set_node_id(allocated); + + PX4_INFO("Allocated Node ID %d", _canard_handle.node_id()); + + } +} + + +void NodeClient::update() +{ + if (hrt_elapsed_time(&_nodealloc_request_last) >= hrt_abstime(2 * + 1000000ULL)) { // Compiler hates me here, some 1_s doesn't work + + int32_t result; + + // Allocation already done, nothing to do + if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) { + return; + } + + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg; + uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE]; + size_t payload_size = PNP2_PAYLOAD_SIZE; + + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + memcpy(node_id_alloc_msg.unique_id, px4_guid, sizeof(node_id_alloc_msg.unique_id)); + //node_id_alloc_msg.node_id.value = preffered_node_id; //FIXME preffered ID PX4 Param + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP2_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _node_id_alloc_transfer_id, + }; + + result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); + } + + } else { + // NodeIDAllocationData message + uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg; + uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg); + uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE]; + size_t payload_size = PNP1_PAYLOAD_SIZE; + + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + node_id_alloc_msg.unique_id_hash = (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF); + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = PNP1_PORT_ID, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _node_id_alloc_transfer_id, + }; + + result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); + } + } + + _nodealloc_request_last = hrt_absolute_time(); + } +} diff --git a/src/drivers/cyphal/NodeClient.hpp b/src/drivers/cyphal/NodeClient.hpp new file mode 100644 index 000000000000..f8a5a6ae1981 --- /dev/null +++ b/src/drivers/cyphal/NodeClient.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 NodeClient.hpp + * + * Defines basic implementation of UAVCAN PNP for dynamic Node ID + * + * @author Peter van der Perk + */ + +#pragma once + + +#include +#include + +#include "CanardInterface.hpp" + +#include +#include +#include + +#include "Services/AccessRequest.hpp" +#include "Services/ListRequest.hpp" + +#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_ +#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_ +#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_ + +class NodeClient : public UavcanBaseSubscriber +{ +public: + NodeClient(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData", + 0), + _canard_handle(handle) { }; + + void subscribe() override + { + + _canard_handle.RxSubscribe(CanardTransferKindMessage, + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + } + + bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg); + bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg); + + void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback + + void update(); + +private: + + CanardHandle &_canard_handle; + CanardTransferID _node_id_alloc_transfer_id{0}; + + hrt_abstime _nodealloc_request_last{0}; +}; diff --git a/src/drivers/cyphal/NodeManager.cpp b/src/drivers/cyphal/NodeManager.cpp new file mode 100644 index 000000000000..87596df3af5c --- /dev/null +++ b/src/drivers/cyphal/NodeManager.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * 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 NodeIDManager.cpp + * + * Defines basic implementation of UAVCAN PNP for dynamic Node ID + * + * @author Peter van der Perk + */ + +#define RETRY_COUNT 10 + +#include "NodeManager.hpp" + +void NodeManager::callback(const CanardRxTransfer &receive) +{ + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { + uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {}; + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + /// do something with the data + HandleNodeIDRequest(node_id_alloc_msg); + + } else { + uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg {}; + size_t msg_size_in_bytes = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload, + &msg_size_in_bytes); + /// do something with the data + HandleNodeIDRequest(node_id_alloc_msg); + } +} + +bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) +{ + if (msg.allocated_node_id.count == 0) { + uint32_t i; + + msg.allocated_node_id.count = 1; + msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET; + + /* Search for an available NodeID to assign */ + for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { + if (i == _canard_handle.node_id()) { + continue; // Don't give our NodeID to a node + + } else if (nodeid_registry[i].node_id == 0) { // Unused + nodeid_registry[i].node_id = i; + memcpy(&nodeid_registry[i].unique_id, &msg.unique_id_hash, 6); + break; + + } else if (memcmp(&nodeid_registry[i].unique_id[0], &msg.unique_id_hash, 6) == 0) { + msg.allocated_node_id.elements[0].value = nodeid_registry[i].node_id; // Existing NodeID + break; + } + } + + nodeid_registry[i].register_setup = false; // Re-instantiate register setup + nodeid_registry[i].register_index = 0; + nodeid_registry[i].retry_count = 0; + + if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) { + + PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); + + uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + size_t payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, + }; + + int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); + } + + _register_request_last = hrt_absolute_time(); + + return result >= 0; + } + } + + return false; +} + +bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg) +{ + //TODO v2 node id + return false; +} + + +void NodeManager::HandleListResponse(const CanardRxTransfer &receive) +{ + uavcan_register_List_Response_1_0 msg; + + size_t register_in_size_bits = receive.payload_size; + uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + if (msg.name.name.count == 0) { + // Index doesn't exist, we've parsed through all registers + for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { + if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) { + nodeid_registry[i].register_setup = true; // Don't update anymore + } + } + + } else { + for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { + if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) { + nodeid_registry[i].register_index++; // Increment index counter for next update() + nodeid_registry[i].register_setup = false; + nodeid_registry[i].retry_count = 0; + } + } + + if (_access_request.setPortId(receive.metadata.remote_node_id, msg.name, NULL)) { //FIXME confirm handler + PX4_INFO("Set portID succesfull"); + + } else { + PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements); + } + } +} +void NodeManager::update() +{ + if (hrt_elapsed_time(&_register_request_last) >= hrt_abstime(2 * + 1000000ULL)) { // Compiler hates me here, some 1_s doesn't work + for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { + if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) { + //Setting up registers + _list_request.getIndex(nodeid_registry[i].node_id, nodeid_registry[i].register_index, this); + nodeid_registry[i].retry_count++; + + if (nodeid_registry[i].retry_count > RETRY_COUNT) { + nodeid_registry[i].register_setup = true; // Don't update anymore + } + } + } + + _register_request_last = hrt_absolute_time(); + } +} diff --git a/src/drivers/cyphal/NodeManager.hpp b/src/drivers/cyphal/NodeManager.hpp new file mode 100644 index 000000000000..af2fb8b62f94 --- /dev/null +++ b/src/drivers/cyphal/NodeManager.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 NodeManager.hpp + * + * Defines basic implementation of UAVCAN PNP for dynamic Node ID + * + * @author Peter van der Perk + */ + +#pragma once + + +#include +#include + +#include "CanardInterface.hpp" + +#include +#include +#include + +#include "Services/AccessRequest.hpp" +#include "Services/ListRequest.hpp" + +#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_ +#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_ +#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_ + +typedef struct { + uint8_t node_id; + uint8_t unique_id[16]; + bool register_setup; + uint16_t register_index; + uint16_t retry_count; +} UavcanNodeEntry; + +class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface +{ +public: + NodeManager(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData", + 0), + _canard_handle(handle), _access_request(handle, pmgr), _list_request(handle) { }; + + void subscribe() override + { + _access_request.subscribe(); + _list_request.subscribe(); + + _canard_handle.RxSubscribe(CanardTransferKindMessage, + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + } + + bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg); + bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg); + + void response_callback(const CanardRxTransfer &receive) override + { + HandleListResponse(receive); + } + void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback + + void HandleListResponse(const CanardRxTransfer &receive); + + void update(); + +private: + CanardHandle &_canard_handle; + CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0}; + UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite + + UavcanAccessServiceRequest _access_request; + UavcanListServiceRequest _list_request; + + bool nodeRegisterSetup = 0; + + hrt_abstime _register_request_last{0}; +}; diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp new file mode 100644 index 000000000000..15a57f3744ed --- /dev/null +++ b/src/drivers/cyphal/ParamManager.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 ParamManager.cpp + * + * Implements basic functionality of UAVCAN parameter management class + * + * @author Jacob Crabill + */ + +#include "ParamManager.hpp" +#include + +bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value) +{ + for (auto ¶m : _uavcan_params) { + if (strcmp(param_name, param.uavcan_name) == 0) { + param_t param_handle = param_find(param.px4_name); + + if (param_handle == PARAM_INVALID) { + return false; + } + + return param.px4_param_to_register_value(param_handle, value); + } + } + + return false; +} + +bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value) +{ + for (auto ¶m : _uavcan_params) { + if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) { + param_t param_handle = param_find(param.px4_name); + + if (param_handle == PARAM_INVALID) { + return false; + } + + return param.px4_param_to_register_value(param_handle, value); + } + } + + return false; +} + +bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name) +{ + if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + return false; + } + + strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + + name.name.count = strlen(_uavcan_params[id].uavcan_name); + + return true; +} + +bool UavcanParamManager::SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value) +{ + for (auto ¶m : _uavcan_params) { + if (strncmp((char *)name.name.elements, param.uavcan_name, name.name.count) == 0) { + param_t param_handle = param_find(param.px4_name); + + if (param_handle == PARAM_INVALID) { + return false; + } + + return param.register_value_to_px4_param(value, param_handle); + } + } + + return false; +} diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp new file mode 100644 index 000000000000..c9c0cd7b62d4 --- /dev/null +++ b/src/drivers/cyphal/ParamManager.hpp @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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 ParamManager.hpp + * + * Defines basic functionality of UAVCAN parameter management class + * + * @author Jacob Crabill + */ + +#pragma once + +#include +#include + +#include +#include + +static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; +static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U; + +static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out) +{ + if (param_type(in) == PARAM_TYPE_INT32) { + int32_t out_val {}; + const int res = param_get(in, &out_val); + + if (res != PX4_OK) { + // Parameter not found / internal error + return false; + } + + if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) { + out.natural16.value.elements[0] = (uint16_t)out_val; + + } else { + // "Invalid" value -- set to "UNSET" + out.natural16.value.elements[0] = CANARD_PORT_ID_UNSET; + } + + out.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(&out); + return true; + } + + return false; +}; + +static bool uavcan_port_id_to_px4_param(const uavcan_register_Value_1_0 &in, param_t &out) +{ + if (uavcan_register_Value_1_0_is_natural16_(&in) && in.natural16.value.count == 1) { + if (param_type(out) == PARAM_TYPE_INT32) { + int32_t in_val = in.natural16.value.elements[0]; + param_set(out, &in_val); + return true; + } + } + + return false; +}; + +using param_2_reg_t = bool(*)(param_t &in, uavcan_register_Value_1_0 &out); +using reg_2_param_t = bool(*)(const uavcan_register_Value_1_0 &in, param_t &out); + + +typedef struct { + const char *uavcan_name; + const char *px4_name; + param_2_reg_t px4_param_to_register_value; + reg_2_param_t register_value_to_px4_param; + bool is_mutable {true}; + bool is_persistent {true}; +} UavcanParamBinder; + + +class UavcanParamManager +{ +public: + + bool GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value); + bool GetParamByName(const uavcan_register_Name_1_0 &name, uavcan_register_Value_1_0 &value); + bool GetParamName(uint32_t id, uavcan_register_Name_1_0 &name); + bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value); + +private: + + + const UavcanParamBinder _uavcan_params[13] { + {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.actuator_outputs.0.id", "UCAN1_ACTR_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.esc.0.id", "UCAN1_ESC0_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.gps.0.id", "UCAN1_GPS0_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.gps.1.id", "UCAN1_GPS1_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.energy_source.0.id", "UCAN1_BMS_ES_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing + //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, + }; +}; diff --git a/src/drivers/cyphal/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp new file mode 100644 index 000000000000..2fd3f745aaf9 --- /dev/null +++ b/src/drivers/cyphal/PublicationManager.cpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 PublicationManager.cpp + * + * Manages the dynamic (run-time configurable) UAVCAN publications + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + + +#include "PublicationManager.hpp" + +PublicationManager::~PublicationManager() +{ + _dynpublishers.clear(); +} + +void PublicationManager::updateDynamicPublications() +{ + for (auto &sub : _uavcan_pubs) { + + bool found_publisher = false; + + for (auto &dynpub : _dynpublishers) { + // Check if subscriber has already been created + char full_subj_name[200]; + snprintf(full_subj_name, sizeof(full_subj_name), "%s%s", dynpub->getPrefixName(), dynpub->getSubjectName()); + const uint8_t instance = dynpub->getInstance(); + + if (strcmp(full_subj_name, sub.subject_name) == 0 && instance == sub.instance) { + found_publisher = true; + break; + } + } + + if (found_publisher) { + continue; + } + + char uavcan_param[90]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s.%d.id", sub.subject_name, sub.instance); + uavcan_register_Value_1_0 value; + + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t port_id = value.natural16.value.elements[0]; + + if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber + UavcanPublisher *dynpub = sub.create_pub(_canard_handle, _param_manager); + + if (dynpub == nullptr) { + PX4_ERR("Out of memory"); + return; + } + + _dynpublishers.add(dynpub); + + dynpub->updateParam(); + } + + } else { + PX4_ERR("Port ID param for publisher %s.%u not found", sub.subject_name, sub.instance); + return; + } + } +} + +void PublicationManager::printInfo() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->printInfo(); + } +} + +void PublicationManager::updateParams() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->updateParam(); + } + + // Check for any newly-enabled publication + updateDynamicPublications(); +} + +void PublicationManager::update() +{ + for (auto &dynpub : _dynpublishers) { + dynpub->update(); + } +} diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp new file mode 100644 index 000000000000..b3d39ffbacd4 --- /dev/null +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -0,0 +1,164 @@ +/**************************************************************************** + * + * 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 PublicationManager.hpp + * + * Manages the dynamic (run-time configurable) UAVCAN publications + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + +#pragma once + +#include + +#ifndef CONFIG_CYPHAL_GNSS_PUBLISHER +#define CONFIG_CYPHAL_GNSS_PUBLISHER 0 +#endif + +#ifndef CONFIG_CYPHAL_ESC_CONTROLLER +#define CONFIG_CYPHAL_ESC_CONTROLLER 0 +#endif + +#ifndef CONFIG_CYPHAL_READINESS_PUBLISHER +#define CONFIG_CYPHAL_READINESS_PUBLISHER 0 +#endif + +#ifndef CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER +#define CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0 +#endif + +#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER +#define CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER 0 +#endif + +/* Preprocessor calculation of publisher count */ + +#define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \ + CONFIG_CYPHAL_ESC_CONTROLLER + \ + CONFIG_CYPHAL_READINESS_PUBLISHER + \ + CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ + CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER + +#include +#include +#include "Publishers/Publisher.hpp" +#include "CanardInterface.hpp" + +#include +#include + +#include "Actuators/EscClient.hpp" +#include "Publishers/udral/Readiness.hpp" +#include "Publishers/udral/Gnss.hpp" +#include "Publishers/uORB/uorb_publisher.hpp" + +typedef struct { + UavcanPublisher *(*create_pub)(CanardHandle &handle, UavcanParamManager &pmgr) {}; + const char *subject_name; + const uint8_t instance; +} UavcanDynPubBinder; + +class PublicationManager +{ +public: + PublicationManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {} + ~PublicationManager(); + + void update(); + void printInfo(); + void updateParams(); + +private: + void updateDynamicPublications(); + + CanardHandle &_canard_handle; + UavcanParamManager &_param_manager; + List _dynpublishers; + + + const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] { +#if CONFIG_CYPHAL_GNSS_PUBLISHER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanGnssPublisher(handle, pmgr, 0); + }, + "gps", + 0 + }, +#endif +#if CONFIG_CYPHAL_ESC_CONTROLLER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanEscController(handle, pmgr); + }, + "esc", + 0 + }, +#endif +#if CONFIG_CYPHAL_READINESS_PUBLISHER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new UavcanReadinessPublisher(handle, pmgr, 0); + }, + "readiness", + 0 + }, +#endif +#if CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new uORB_over_UAVCAN_Publisher(handle, pmgr, ORB_ID(actuator_outputs)); + }, + "uorb.actuator_outputs", + 0 + }, +#endif +#if CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new uORB_over_UAVCAN_Publisher(handle, pmgr, ORB_ID(sensor_gps)); + }, + "uorb.sensor_gps", + 0 + }, +#endif + }; +}; diff --git a/src/drivers/cyphal/Publishers/Publisher.hpp b/src/drivers/cyphal/Publishers/Publisher.hpp new file mode 100644 index 000000000000..cb04ef5fdd82 --- /dev/null +++ b/src/drivers/cyphal/Publishers/Publisher.hpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * 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 Publisher.hpp + * + * Defines basic functionality of Cyphal publisher class + * + * @author Jacob Crabill + */ + +#pragma once + +#include + +#include +#include + +#include + +#include "../CanardHandle.hpp" +#include "../CanardInterface.hpp" +#include "../ParamManager.hpp" + +/* This is a default baseline timeout for publishers + * Still it's recommended for implementers to check if their publisher + * has timing requirements and if it should drop messages that are too old in favour of newer messages + */ +#define PUBLISHER_DEFAULT_TIMEOUT_USEC 100000UL + +class UavcanPublisher : public ListNode +{ +public: + UavcanPublisher(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name, + uint8_t instance = 0) : + _canard_handle(handle), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name), + _instance(instance) { }; + + virtual ~UavcanPublisher() = default; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() = 0; + + CanardPortID id() { return _port_id; }; + + void updateParam() + { + char uavcan_param[256]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.pub.%s%s.%d.id", _prefix_name, _subject_name, _instance); + + // Set _port_id from _uavcan_param + uavcan_register_Value_1_0 value; + + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t new_id = value.natural16.value.elements[0]; + + if (_port_id != new_id) { + if (new_id == CANARD_PORT_ID_UNSET) { + PX4_INFO("Disabling publication of subject %s%s.%d", _prefix_name, _subject_name, _instance); + _port_id = CANARD_PORT_ID_UNSET; + + } else { + _port_id = (CanardPortID)new_id; + PX4_INFO("Enabling subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id); + } + } + } + }; + + void printInfo() + { + if (_port_id != CANARD_PORT_ID_UNSET) { + PX4_INFO("Enabled subject %s%s.%d on port %d", _prefix_name, _subject_name, _instance, _port_id); + + } else { + PX4_INFO("Subject %s%s.%d disabled", _prefix_name, _subject_name, _instance); + } + } + + const char *getSubjectName() + { + return _subject_name; + } + + const char *getPrefixName() + { + return _prefix_name; + } + + uint8_t getInstance() + { + return _instance; + } + + UavcanPublisher *next() + { + return _next_pub; + } + + void setNext(UavcanPublisher *next) + { + _next_pub = next; + } + +protected: + CanardHandle &_canard_handle; + UavcanParamManager &_param_manager; + const char *_prefix_name; + const char *_subject_name; + uint8_t _instance {0}; + + CanardPortID _port_id {CANARD_PORT_ID_UNSET}; + CanardTransferID _transfer_id {0}; + + UavcanPublisher *_next_pub {nullptr}; +}; diff --git a/src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp new file mode 100644 index 000000000000..590f123b5bab --- /dev/null +++ b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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 uorb_template.cpp + * +* Defines generic, templatized uORB over UAVCANv1 publisher + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + +#include "uorb_publisher.hpp" + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + +template<> +size_t uORB_over_UAVCAN_Publisher::get_payload_size(actuator_outputs_s *msg) +{ + // Remove unvalid output & padding from payload_size to save bandwidth + return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) - + ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0])); +} diff --git a/src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp new file mode 100644 index 000000000000..3843737e35a2 --- /dev/null +++ b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 uorb_template.hpp + * +* Defines generic, templatized uORB over UAVCANv1 publisher + * + * @author Peter van der Perk + */ + +#pragma once + +#include "../Publisher.hpp" + +#include +#include + +template +class uORB_over_UAVCAN_Publisher : public UavcanPublisher +{ +public: + uORB_over_UAVCAN_Publisher(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta, + uint8_t instance = 0) : + UavcanPublisher(handle, pmgr, "uorb.", meta->o_name, instance), + _uorb_meta{meta}, + _uorb_sub(meta) + {}; + + ~uORB_over_UAVCAN_Publisher() override = default; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() override + { + // Not sure if actuator_armed is a good indication of readiness but seems close to it + if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + T data {}; + _uorb_sub.update(&data); + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id + }; + + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + get_payload_size(&data), + &data); + } + }; + +protected: + // Default payload-size function -- can specialize in derived class + size_t get_payload_size(T *msg) + { + (void)msg; + return sizeof(T); + } + +private: + const orb_metadata *_uorb_meta; + uORB::Subscription _uorb_sub; +}; + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + +template<> +size_t uORB_over_UAVCAN_Publisher::get_payload_size(actuator_outputs_s *msg); diff --git a/src/drivers/cyphal/Publishers/udral/Gnss.hpp b/src/drivers/cyphal/Publishers/udral/Gnss.hpp new file mode 100644 index 000000000000..168b1e437f1a --- /dev/null +++ b/src/drivers/cyphal/Publishers/udral/Gnss.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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 Gnss.hpp + * + * Defines basic functionality of Cyphal GNSS publisher + * + * @author Jacob Crabill + */ + +#pragma once + +// UDRAL Specification Messages +#include + +#include "../Publisher.hpp" + +class UavcanGnssPublisher : public UavcanPublisher +{ +public: + UavcanGnssPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(handle, pmgr, "udral", "gps", instance) + { + + }; + + ~UavcanGnssPublisher() override = default; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() override + { + if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + sensor_gps_s gps {}; + _gps_sub.update(&gps); + size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; + + reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; + geo.latitude = gps.lat; + geo.longitude = gps.lon; + geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast(gps.alt) }; + + uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + }; + + int32_t result = reg_udral_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &geo_payload_buffer); + } + } + }; + +private: + + /// TODO: Allow >1 instance + uORB::Subscription _gps_sub{ORB_ID(sensor_gps)}; + CanardTransferID _transfer_id_2 {0}; +}; diff --git a/src/drivers/cyphal/Publishers/udral/Readiness.hpp b/src/drivers/cyphal/Publishers/udral/Readiness.hpp new file mode 100644 index 000000000000..8afe59f34a2e --- /dev/null +++ b/src/drivers/cyphal/Publishers/udral/Readiness.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 Readiness.hpp + * + * Defines the Cyphal readiness publisher + * readiness state is used to command or report the availability status + * + * @author Peter van der Perk + */ + +#pragma once + +// UDRAL Specification Messages +#include + +#include "../Publisher.hpp" + +class UavcanReadinessPublisher : public UavcanPublisher +{ +public: + UavcanReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(handle, pmgr, "udral", "readiness", instance) + { + + }; + + ~UavcanReadinessPublisher() override = default; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() override + { + // Not sure if actuator_armed is a good indication of readiness but seems close to it + if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + actuator_armed_s armed {}; + _actuator_armed_sub.update(&armed); + size_t payload_size; + + reg_udral_service_common_Readiness_0_1 readiness {}; + + if (armed.armed) { + readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED; + + } else { + readiness.value = reg_udral_service_common_Readiness_0_1_STANDBY; + } + + uint8_t readiness_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + }; + + int32_t result = reg_udral_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer, + &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &readiness_payload_buffer); + } + } + }; + +private: + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; +}; diff --git a/src/drivers/cyphal/ServiceClients/Access.hpp b/src/drivers/cyphal/ServiceClients/Access.hpp new file mode 100644 index 000000000000..824c0514076a --- /dev/null +++ b/src/drivers/cyphal/ServiceClients/Access.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 Access.hpp + * + * Defines response to a Access request + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../ParamManager.hpp" + +#include +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanAccessResponse : public UavcanBaseSubscriber +{ +public: + UavcanAccessResponse(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(handle, "", "Access", 0), _param_manager(pmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + _canard_handle.RxSubscribe(CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + }; + + void callback(const CanardRxTransfer &receive) override + { + PX4_INFO("Access request"); + + size_t payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + + uavcan_register_Access_Request_1_0 msg; + uavcan_register_Access_Request_1_0_initialize_(&msg); + + size_t register_in_size_bits = receive.payload_size; + uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + int result {0}; + + uavcan_register_Value_1_0 value = msg.value; + uavcan_register_Name_1_0 name = msg.name; + + /// TODO: get/set parameter based on whether empty or not + if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0 + // Value is empty -- 'Get' only + result = _param_manager.GetParamByName(name, value) ? 0 : -1; + + } else { + // Set value + result = _param_manager.SetParamByName(name, value) ? 0 : -1; + + } + + /// TODO: Access_Response + uavcan_register_Access_Response_1_0 response {}; + response.value = value; + + uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = receive.metadata.transfer_id, + }; + + result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); + } + + //return result; + + }; + +private: + UavcanParamManager &_param_manager; + +}; diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/cyphal/ServiceClients/GetInfo.hpp similarity index 77% rename from src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp rename to src/drivers/cyphal/ServiceClients/GetInfo.hpp index bc24004e2e0e..803128cf0548 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/cyphal/ServiceClients/GetInfo.hpp @@ -45,40 +45,38 @@ #include #include -#include "../NodeManager.hpp" - #include #include #include "../Subscribers/BaseSubscriber.hpp" +#include "../Publishers/Publisher.hpp" class UavcanGetInfoResponse : public UavcanBaseSubscriber { public: - UavcanGetInfoResponse(CanardInstance &ins) : - UavcanBaseSubscriber(ins, "GetInfo", 0) { }; + UavcanGetInfoResponse(CanardHandle &handle) : + UavcanBaseSubscriber(handle, "", "GetInfo", 0) { }; void subscribe() override { // Subscribe to requests uavcan.pnp.NodeIDAllocationData - canardRxSubscribe(&_canard_instance, - CanardTransferKindRequest, - uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, - uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_canard_sub); - - _port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_; + _canard_handle.RxSubscribe( + CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("GetInfo request"); // Setup node.GetInfo response uavcan_node_GetInfo_Response_1_0 node_info; + size_t payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; uavcan_node_GetInfo_Response_1_0_initialize_(&node_info); @@ -109,26 +107,23 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardMicrosecond transmission_deadline = hrt_absolute_time() + 1000 * 100; - - CanardTransfer response = { - .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindResponse, .port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Send back to request Node - .transfer_id = getinfo_response_transfer_id, - .payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &response_payload_buffer, + .remote_node_id = receive.metadata.remote_node_id, // Send back to request Node + .transfer_id = receive.metadata.transfer_id }; int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer, - &response.payload_size); + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed - ++getinfo_response_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &response); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); } //TODO proper error handling @@ -141,7 +136,4 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber }; -private: - CanardTransferID getinfo_response_transfer_id = 0; - }; diff --git a/src/drivers/cyphal/ServiceClients/List.hpp b/src/drivers/cyphal/ServiceClients/List.hpp new file mode 100644 index 000000000000..104006739144 --- /dev/null +++ b/src/drivers/cyphal/ServiceClients/List.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 List.hpp + * + * Defines response to a List request + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include "../ParamManager.hpp" + +#include + +#include "../Subscribers/BaseSubscriber.hpp" + +class UavcanListResponse : public UavcanBaseSubscriber +{ +public: + UavcanListResponse(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(handle, "", "List", 0), _param_manager(pmgr) { }; + + void subscribe() override + { + // Subscribe to requests uavcan.pnp.NodeIDAllocationData + + _canard_handle.RxSubscribe(CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + }; + + void callback(const CanardRxTransfer &receive) override + { + PX4_INFO("List request"); + + size_t payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + + uavcan_register_List_Request_1_0 msg; + uavcan_register_List_Response_1_0 response; + + uavcan_register_List_Request_1_0_initialize_(&msg); + uavcan_register_List_Response_1_0_initialize_(&response); + + size_t register_in_size_bits = receive.payload_size; + uavcan_register_List_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, ®ister_in_size_bits); + + int result {0}; + + if (_param_manager.GetParamName(msg.index, response.name) == 0) { + response.name.name.count = 0; + } + + uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindResponse, + .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = receive.metadata.transfer_id + }; + + result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); + } + + }; + +private: + UavcanParamManager &_param_manager; + +}; diff --git a/src/drivers/cyphal/Services/AccessRequest.hpp b/src/drivers/cyphal/Services/AccessRequest.hpp new file mode 100644 index 000000000000..f162d24e6854 --- /dev/null +++ b/src/drivers/cyphal/Services/AccessRequest.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 Access.hpp + * + * Defines a Access Service invoker and process Access responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include + +#include "ServiceRequest.hpp" +#include "../ParamManager.hpp" +#include "../Publishers/Publisher.hpp" + +class UavcanAccessServiceRequest : public UavcanServiceRequest +{ +public: + UavcanAccessServiceRequest(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanServiceRequest(handle, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_), _param_manager(pmgr) { }; + + bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler) + { + int result {0}; + + uavcan_register_Access_Request_1_0 request_msg; + request_msg.value.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type + + //FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis + name.name.elements[7] = 's'; //HACK Change pub into sub + + if (_param_manager.GetParamByName(name, request_msg.value)) { + size_t payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + name.name.elements[7] = 'p'; //HACK Change sub into pub + memcpy(&request_msg.name, &name, sizeof(request_msg.name)); + + uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = _portID, // This is the subject-ID. + .remote_node_id = node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = request_transfer_id + }; + + result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &payload_size); + + if (result == 0) { + return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &request_payload_buffer, + handler); + + } else { + return false; + } + + } else { + return false; + } + }; + +private: + UavcanParamManager &_param_manager; + +}; diff --git a/src/drivers/cyphal/Services/ListRequest.hpp b/src/drivers/cyphal/Services/ListRequest.hpp new file mode 100644 index 000000000000..f5bad790d0a6 --- /dev/null +++ b/src/drivers/cyphal/Services/ListRequest.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 ListRequest.hpp + * + * Defines a List Service invoker and process List responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include + +#include "ServiceRequest.hpp" +#include "../ParamManager.hpp" +#include "../Publishers/Publisher.hpp" + +class UavcanListServiceRequest : public UavcanServiceRequest +{ +public: + UavcanListServiceRequest(CanardHandle &handle) : + UavcanServiceRequest(handle, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_) { }; + + + bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler) + { + uavcan_register_List_Request_1_0 msg; + size_t payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + msg.index = index; + + uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = node_id, + .transfer_id = request_transfer_id + }; + + if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &payload_size) == 0) { + return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &request_payload_buffer, + handler); + + } else { + return false; + } + }; + + +}; diff --git a/src/drivers/cyphal/Services/ServiceRequest.hpp b/src/drivers/cyphal/Services/ServiceRequest.hpp new file mode 100644 index 000000000000..b636ab75dfe7 --- /dev/null +++ b/src/drivers/cyphal/Services/ServiceRequest.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 ServiceRequest.hpp + * + * Defines a Service invoker base class and process responses + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include +#include + +#include + +#include "../Subscribers/BaseSubscriber.hpp" + + +class UavcanServiceRequestInterface +{ +public: + virtual void response_callback(const CanardRxTransfer &receive) = 0; +}; + +class UavcanServiceRequest : public UavcanBaseSubscriber +{ +public: + UavcanServiceRequest(CanardHandle &handle, const char *prefix_name, const char *subject_name, CanardPortID portID, + size_t extent) : + UavcanBaseSubscriber(handle, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { }; + + + void subscribe() override + { + // Subscribe to requests response + _canard_handle.RxSubscribe(CanardTransferKindResponse, + _portID, + _extent, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + }; + + bool request(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *transfer_metadata, + const size_t payload_size, + const void *const payload, + UavcanServiceRequestInterface *handler) + { + _response_callback = handler; + remote_node_id = transfer_metadata->remote_node_id; + ++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + return _canard_handle.TxPush(tx_deadline_usec, + transfer_metadata, + payload_size, + payload) > 0; + } + + void callback(const CanardRxTransfer &receive) override + { + PX4_INFO("Response"); + + if (_response_callback != nullptr && + receive.metadata.transfer_id == (request_transfer_id - 1) && + receive.metadata.remote_node_id == remote_node_id) { + _response_callback->response_callback(receive); + } + }; + + + +protected: + CanardTransferID request_transfer_id = 0; + CanardNodeID remote_node_id = CANARD_NODE_ID_UNSET; + + const CanardPortID _portID; + const size_t _extent; + UavcanServiceRequestInterface *_response_callback = nullptr; + +}; diff --git a/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp b/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp new file mode 100644 index 000000000000..a673bd046bc7 --- /dev/null +++ b/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * 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 BaseSubscriber.hpp + * + * Defines basic functionality of Cyphal subscriber class + * + * @author Jacob Crabill + */ + +#pragma once + +#include + +#include + +#include "../CanardHandle.hpp" +#include "../CanardInterface.hpp" +#include "../ParamManager.hpp" + +class UavcanBaseSubscriber +{ +public: + UavcanBaseSubscriber(CanardHandle &handle, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : + _canard_handle(handle), _prefix_name(prefix_name), _instance(instance) + { + _subj_sub._subject_name = subject_name; + _subj_sub._canard_sub.user_reference = this; + _subj_sub._canard_sub.port_id = CANARD_PORT_ID_UNSET; + } + + virtual ~UavcanBaseSubscriber() + { + unsubscribe(); + } + + bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; } + + virtual void subscribe() = 0; + virtual void unsubscribe() + { + SubjectSubscription *curSubj = &_subj_sub; + + while (curSubj != nullptr) { + _canard_handle.RxUnsubscribe(CanardTransferKindMessage, curSubj->_canard_sub.port_id); + curSubj = curSubj->next; + } + }; + + virtual void callback(const CanardRxTransfer &msg) = 0; + + CanardPortID id(uint32_t instance = 0) + { + uint32_t i = 0; + SubjectSubscription *curSubj = &_subj_sub; + + while (curSubj != nullptr) { + if (instance == i) { + return curSubj->_canard_sub.port_id; + } + + curSubj = curSubj->next; + i++; + } + + return CANARD_PORT_ID_UNSET; // Wrong id return unset + } + + bool hasPortID(CanardPortID port_id) + { + if (!isValidPortId((int32_t)port_id)) { + return false; + } + + SubjectSubscription *curSubj = &_subj_sub; + + while (curSubj != nullptr) { + if (port_id == curSubj->_canard_sub.port_id) { + return true; + } + + curSubj = curSubj->next; + } + + return false; + } + + const char *getSubjectName() + { + return _subj_sub._subject_name; + } + + uint8_t getInstance() + { + return _instance; + } + + void printInfo() + { + SubjectSubscription *curSubj = &_subj_sub; + + while (curSubj != nullptr) { + if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { + PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id); + } + + curSubj = curSubj->next; + } + } + +protected: + struct SubjectSubscription { + CanardRxSubscription _canard_sub; + const char *_subject_name; + struct SubjectSubscription *next {nullptr}; + }; + + CanardHandle &_canard_handle; + const char *_prefix_name; + SubjectSubscription _subj_sub; + uint8_t _instance {0}; + /// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan) +}; diff --git a/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp new file mode 100644 index 000000000000..dd417c99673c --- /dev/null +++ b/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 DynamicPortSubscriber.hpp + * + * Defines basic functionality of Cyphal subscriber class with Non-fixed unregulated port identifier + * + * @author Jacob Crabill + */ + +#pragma once + +#include + +#include + +#include + +#include "../CanardInterface.hpp" +#include "../ParamManager.hpp" +#include "BaseSubscriber.hpp" + +class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber +{ +public: + UavcanDynamicPortSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, + const char *subject_name, uint8_t instance = 0) : + UavcanBaseSubscriber(handle, prefix_name, subject_name, instance), _param_manager(pmgr) { }; + + void updateParam() + { + SubjectSubscription *curSubj = &_subj_sub; + + while (curSubj != nullptr) { + char uavcan_param[90]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s%s.%d.id", _prefix_name, curSubj->_subject_name, _instance); + + // Set _port_id from _uavcan_param + uavcan_register_Value_1_0 value; + + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t new_id = value.natural16.value.elements[0]; + + /* FIXME how about partial subscribing */ + if (curSubj->_canard_sub.port_id != new_id) { + if (new_id == CANARD_PORT_ID_UNSET) { + // Cancel subscription + unsubscribe(); + + } else { + if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { + // Already active; unsubscribe first + unsubscribe(); + } + + // Subscribe on the new port ID + curSubj->_canard_sub.port_id = (CanardPortID)new_id; + PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance, + curSubj->_canard_sub.port_id); + subscribe(); + } + } + + } else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary + // Already active; unsubscribe first + unsubscribe(); + } + + curSubj = curSubj->next; + } + }; + + UavcanDynamicPortSubscriber *next() + { + return _next_sub; + } + + void setNext(UavcanDynamicPortSubscriber *next) + { + _next_sub = next; + } + +protected: + UavcanParamManager &_param_manager; + UavcanDynamicPortSubscriber *_next_sub {nullptr}; +}; diff --git a/src/drivers/cyphal/Subscribers/Heartbeat.hpp b/src/drivers/cyphal/Subscribers/Heartbeat.hpp new file mode 100644 index 000000000000..9001b9386117 --- /dev/null +++ b/src/drivers/cyphal/Subscribers/Heartbeat.hpp @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * 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 Heartbeat.hpp + * + * Defines basic functionality of UAVCAN Heartbeat.1.0 subscription + * + * @author Peter van der Perk + */ + +#pragma once + +#include "../NodeManager.hpp" + +#include + +#include "BaseSubscriber.hpp" + +class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber +{ +public: + UavcanHeartbeatSubscriber(CanardHandle &handle) : + UavcanBaseSubscriber(handle, "", "Heartbeat", 0) { }; + + void subscribe() override + { + // Subscribe to heartbeat messages + _canard_handle.RxSubscribe(CanardTransferKindMessage, + uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID + uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + }; + + void callback(const CanardRxTransfer &receive) override + { + //TODO heartbeat management + }; + +}; diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp new file mode 100644 index 000000000000..4a4d316ee09c --- /dev/null +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 Battery.hpp + * + * Defines basic functionality of UAVCAN legacy Battery subscription + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include + +// Legacy message from UAVCANv0 +#include + +#include "../DynamicPortSubscriber.hpp" + +class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanLegacyBatteryInfoSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "legacy.", "legacy_bms", instance) { }; + + void subscribe() override + { + // Subscribe to messages reg.drone.service.battery.Status.0.1 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler + &_subj_sub._canard_sub); + }; + + void callback(const CanardRxTransfer &receive) override + { + PX4_INFO("Legacy BmsCallback"); + + legacy_equipment_power_BatteryInfo_1_0 bat_info {}; + size_t bat_info_size_in_bytes = receive.payload_size; + legacy_equipment_power_BatteryInfo_1_0_deserialize_(&bat_info, (const uint8_t *)receive.payload, + &bat_info_size_in_bytes); + + battery_status_s bat_status {0}; + bat_status.timestamp = hrt_absolute_time(); + bat_status.voltage_filtered_v = bat_info.voltage; + bat_status.current_filtered_a = bat_info.current; + bat_status.current_average_a = bat_info.average_power_10sec; + bat_status.remaining = bat_info.state_of_charge_pct / 100.0f; + bat_status.scale = -1; + + if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_HOT) { + bat_status.temperature = 100; + + } else if (bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_TEMP_COLD) { + bat_status.temperature = -30; + + } else { + bat_status.temperature = 20; // Temp okay ? + } + + bat_status.cell_count = 0; // Unknown + bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE; + bat_status.source = 1; // External + bat_status.capacity = bat_info.full_charge_capacity_wh; + bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits + bat_status.state_of_health = bat_info.state_of_health_pct; // External + bat_status.id = bat_info.battery_id; + + /* Missing fields in UAVCANv0 legacy message + * temperature (partly) + * cell_count + * connected (partly) + * priority + * cycle_count + * time_remaining_s + * average_time_to_empty + * manufacture_date + * max_error + * interface_error + * voltage_cell_v + * max_cell_voltage_delta + * is_powering_off + * warning + */ + + + _battery_status_pub.publish(bat_status); + print_message(ORB_ID(battery_status), bat_status); + }; + +private: + uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; + +}; diff --git a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp new file mode 100644 index 000000000000..87a3c2d39f6c --- /dev/null +++ b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 uorb_template.cpp + * +* Defines generic, templatized uORB over UAVCANv1 subscriber + * + * @author Peter van der Perk + * @author Jacob Crabill + */ + +#include "uorb_subscriber.hpp" + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + +/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */ + +template<> +void uORB_over_UAVCAN_Subscriber::convert(sensor_gps_s *data) +{ + /* HOTFIX as long as we don't have UAVCAN timesyncronization we update the timestamp on arrival */ + data->timestamp = hrt_absolute_time(); +} diff --git a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp new file mode 100644 index 000000000000..a80edf292d75 --- /dev/null +++ b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp @@ -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 uorb_template.hpp + * +* Defines generic, templatized uORB over UAVCANv1 publisher + * + * @author Peter van der Perk + */ + +#pragma once + +#include "../DynamicPortSubscriber.hpp" + +#include +#include + +#include + +template +class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber +{ +public: + uORB_over_UAVCAN_Subscriber(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta, + uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "uorb.", meta->o_name, instance), + _uorb_meta{meta}, + _uorb_pub(meta) + {}; + + ~uORB_over_UAVCAN_Subscriber() override = default; + + void subscribe() override + { + T *data = NULL; + + // Subscribe to messages uORB sensor_gps payload over UAVCAN + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + get_payload_size(data), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000, + &_subj_sub._canard_sub); + }; + + void callback(const CanardRxTransfer &receive) override + { + T *data = (T *)receive.payload; + + if (receive.payload_size == get_payload_size(data)) { + + /* Data type specific conversion if necceary */ + convert(data); + + _uorb_pub.publish(*data); + + } else { + PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d", + _subj_sub._subject_name, receive.payload_size, get_payload_size(data)); + } + }; + +protected: + // Default payload-size function -- can specialize in derived class + size_t get_payload_size(const T *msg) + { + (void)msg; + return sizeof(T); + }; + + void convert(T *data) {}; + +private: + const orb_metadata *_uorb_meta; + uORB::PublicationMulti _uorb_pub; +}; + +/* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ + + +/* ---- Specializations of convert() to convert incompatbile data, instance no. timestamp ---- */ + +template<> +void uORB_over_UAVCAN_Subscriber::convert(sensor_gps_s *data); diff --git a/src/drivers/cyphal/Subscribers/udral/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp new file mode 100644 index 000000000000..732c3dc2e88a --- /dev/null +++ b/src/drivers/cyphal/Subscribers/udral/Battery.hpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * 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 Battery.hpp + * + * Defines basic functionality of Cyphal Battery subscription + * + * @author Jacob Crabill + */ + +#pragma once + +#include +#include + +// UDRAL Specification Messages +#include +#include +#include + +#include "../DynamicPortSubscriber.hpp" + +#define KELVIN_OFFSET 273.15f +#define WH_TO_JOULE 3600 + +class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanBmsSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "energy_source", instance) + { + _subj_sub.next = &_status_sub; + + _status_sub._subject_name = _status_name; + _status_sub._canard_sub.user_reference = this; + _status_sub.next = &_parameters_sub; + + _parameters_sub._subject_name = _parameters_name; + _parameters_sub._canard_sub.user_reference = this; + _parameters_sub.next = nullptr; + } + + void subscribe() override + { + // Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_physics_electricity_SourceTs_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + // Subscribe to messages reg.drone.service.battery.Status.0.2 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _status_sub._canard_sub.port_id, + reg_udral_service_battery_Status_0_2_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_status_sub._canard_sub); + + // Subscribe to messages reg.drone.service.battery.Parameters.0.3 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _parameters_sub._canard_sub.port_id, + reg_udral_service_battery_Parameters_0_3_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_parameters_sub._canard_sub); + }; + + void callback(const CanardRxTransfer &receive) override + { + if (receive.metadata.port_id == _subj_sub._canard_sub.port_id) { + reg_udral_physics_electricity_SourceTs_0_1 source_ts {}; + size_t source_ts_size_in_bytes = receive.payload_size; + reg_udral_physics_electricity_SourceTs_0_1_deserialize_(&source_ts, + (const uint8_t *)receive.payload, + &source_ts_size_in_bytes); + bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond; + bat_status.voltage_v = source_ts.value.power.voltage.volt; + bat_status.current_a = source_ts.value.power.current.ampere; + + bat_status.connected = true; // Based on some threshold or an error?? + + // Estimate discharged mah from Joule + if (_nominal_voltage != NAN) { + bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule) + / (_nominal_voltage * WH_TO_JOULE); + } + + bat_status.remaining = source_ts.value.energy.joule / source_ts.value.full_energy.joule; + + // TODO uORB publication rate limiting + _battery_status_pub.publish(bat_status); + + } else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) { + reg_udral_service_battery_Status_0_2 bat {}; + size_t bat_size_in_bytes = receive.payload_size; + reg_udral_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes); + + bat_status.scale = -1; // What does the mean? + + bat_status.temperature = bat.temperature_min_max[1].kelvin - KELVIN_OFFSET; // PX4 uses degC we assume + + bat_status.cell_count = bat.cell_voltages.count; + + uint32_t cell_count = bat_status.cell_count; + + if (cell_count > (sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]))) { + cell_count = sizeof(bat_status.voltage_cell_v) / sizeof(bat_status.voltage_cell_v[0]); + } + + float voltage_cell_min = FLT_MAX_EXP; + float voltage_cell_max = FLT_MIN_EXP; + + for (uint32_t i = 0; i < cell_count; i++) { + bat_status.voltage_cell_v[i] = bat.cell_voltages.elements[i]; + + if (bat_status.voltage_cell_v[i] > voltage_cell_max) { + voltage_cell_max = bat_status.voltage_cell_v[i]; + } + + if (bat_status.voltage_cell_v[i] < voltage_cell_min) { + voltage_cell_min = bat_status.voltage_cell_v[i]; + } + } + + bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time? + + } else if (receive.metadata.port_id == _parameters_sub._canard_sub.port_id) { + reg_udral_service_battery_Parameters_0_3 parameters {}; + size_t parameters_size_in_bytes = receive.payload_size; + reg_udral_service_battery_Parameters_0_3_deserialize_(¶meters, + (const uint8_t *)receive.payload, + ¶meters_size_in_bytes); + + bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh + bat_status.cycle_count = parameters.cycle_count; + bat_status.serial_number = parameters.unique_id & 0xFFFF; + bat_status.state_of_health = parameters.state_of_health_pct; + bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic + bat_status.id = 0; //TODO instancing + _nominal_voltage = parameters.nominal_voltage.volt; + } + } + +private: + float _nominal_voltage = NAN; + + uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; + + SubjectSubscription _status_sub; + SubjectSubscription _parameters_sub; + + const char *_status_name = "battery_status"; + const char *_parameters_name = "battery_parameters"; + + battery_status_s bat_status {0}; +}; diff --git a/src/drivers/cyphal/Subscribers/udral/Esc.hpp b/src/drivers/cyphal/Subscribers/udral/Esc.hpp new file mode 100644 index 000000000000..7ccfea448aac --- /dev/null +++ b/src/drivers/cyphal/Subscribers/udral/Esc.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 Esc.hpp + * + * Defines basic functionality of Cyphal ESC setpoint subscription + * (For use on a CAN-->PWM node) + * + * @author Jacob Crabill + */ + +#pragma once + +/// For use with PR-16808 once merged +// #include + +// UDRAL Specification Messages +#include +#include + +#include "../DynamicPortSubscriber.hpp" + +class UavcanEscSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanEscSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "esc", instance) { }; + + void subscribe() override + { + // Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + // Subscribe to messages reg.drone.service.common.Readiness.0.1 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + static_cast(static_cast(_subj_sub._canard_sub.port_id) + 1), + reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub_readiness); + }; + + void callback(const CanardRxTransfer &receive) override + { + // Test with Yakut: + // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" + // yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}' + PX4_INFO("EscCallback"); + + reg_udral_service_actuator_common_sp_Vector8_0_1 esc {}; + size_t esc_size_in_bits = receive.payload_size; + reg_udral_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload, + &esc_size_in_bits); + + double val1 = static_cast(esc.value[0]); + double val2 = static_cast(esc.value[1]); + double val3 = static_cast(esc.value[2]); + double val4 = static_cast(esc.value[3]); + PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4); + /// do something with the data + + /// For use with PR-16808 once merged + // output_control_s outputs; + + // for (uint8_t i = 0; i < 8; i++) { + // outputs.value[i] = 2.f * (esc.value[i] / 8191.f) - 1.f; + // } + + // _output_pub.publish(outputs); + }; + +private: + /// For use with PR-16808 once merged + // uORB::Publication _output_pub{ORB_ID(output_control_mc)}; + + CanardRxSubscription _canard_sub_readiness; +}; diff --git a/src/drivers/cyphal/Subscribers/udral/Gnss.hpp b/src/drivers/cyphal/Subscribers/udral/Gnss.hpp new file mode 100644 index 000000000000..589b967a84b0 --- /dev/null +++ b/src/drivers/cyphal/Subscribers/udral/Gnss.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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 Gnss.hpp + * + * Defines basic functionality of Cyphal GNSS subscription + * + * @author Jacob Crabill + */ + +#pragma once + +// UDRAL Specification Messages +#include + +#include "../DynamicPortSubscriber.hpp" + +class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanGnssSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "gps", instance) { }; + + void subscribe() override + { + // Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1 + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + + /** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan): + * # A compliant implementation of this service should publish the following subjects: + * # + * # PUBLISHED SUBJECT NAME SUBJECT TYPE TYP. RATE [Hz] + * # point_kinematics reg.drone.physics.kinematics.geodetic.PointStateVarTs 1...100 + * # time reg.drone.service.gnss.Time 1...10 + * # heartbeat reg.drone.service.gnss.Heartbeat ~1 + * # sensor_status reg.drone.service.sensor.Status ~1 + * + * Not mentioned, but should also be included: Dilution of Precision + * (reg.drone.service.gnss.DilutionOfPrecision.0.1.uavcan) + * For PX4, only the PointStateVarTs, DilutionOfPrecision, and perhaps Time would be needed + * to publish 'sensor_gps' + */ + }; + + void callback(const CanardRxTransfer &receive) override + { + // Test with Yakut: + // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" + // yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}' + PX4_INFO("GpsCallback"); + + reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; + size_t geo_size_in_bits = receive.payload_size; + reg_udral_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits); + + double lat = geo.latitude; + double lon = geo.longitude; + double alt = geo.altitude.meter; + PX4_INFO("Latitude: %f, Longitude: %f, Altitude: %f", lat, lon, alt); + /// do something with the data + }; + +}; diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp new file mode 100644 index 000000000000..3f50850f0f69 --- /dev/null +++ b/src/drivers/cyphal/SubscriptionManager.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * 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 SubscriptionManager.cpp + * + * Manages the UAVCAN subscriptions + * + * @author Peter van der Perk + */ + + +#include "SubscriptionManager.hpp" + +#include "ParamManager.hpp" + +SubscriptionManager::~SubscriptionManager() +{ + UavcanDynamicPortSubscriber *dynsub; + + while (_dynsubscribers != nullptr) { + dynsub = _dynsubscribers; + _dynsubscribers = dynsub->next(); + delete dynsub; + } +} + +void SubscriptionManager::subscribe() +{ + _heartbeat_sub.subscribe(); + +#if CONFIG_CYPHAL_GETINFO_RESPONDER + _getinfo_rsp.subscribe(); +#endif + + _access_rsp.subscribe(); + _list_rsp.subscribe(); + + updateDynamicSubscriptions(); +} + +void SubscriptionManager::updateDynamicSubscriptions() +{ + for (auto &sub : _uavcan_subs) { + + bool found_subscriber = false; + UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; + + while (dynsub != nullptr) { + // Check if subscriber has already been created + const char *subj_name = dynsub->getSubjectName(); + const uint8_t instance = dynsub->getInstance(); + + if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) { + found_subscriber = true; + break; + } + + dynsub = dynsub->next(); + } + + if (found_subscriber) { + continue; + } + + char uavcan_param[90]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", sub.subject_name, sub.instance); + uavcan_register_Value_1_0 value; + + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t port_id = value.natural16.value.elements[0]; + + if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber + dynsub = sub.create_sub(_canard_handle, _param_manager); + + if (dynsub == nullptr) { + PX4_ERR("Out of memory"); + return; + } + + if (_dynsubscribers == nullptr) { + // Set the head of our linked list + _dynsubscribers = dynsub; + + } else { + // Append the new subscriber to our linked list + UavcanDynamicPortSubscriber *tmp = _dynsubscribers; + + while (tmp->next() != nullptr) { + tmp = tmp->next(); + } + + tmp->setNext(dynsub); + } + + dynsub->updateParam(); + } + + } else { + PX4_ERR("Port ID param for subscriber %s.%u not found", sub.subject_name, sub.instance); + return; + } + } +} + +void SubscriptionManager::printInfo() +{ + UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; + + while (dynsub != nullptr) { + dynsub->printInfo(); + dynsub = dynsub->next(); + } +} + +void SubscriptionManager::updateParams() +{ + UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; + + while (dynsub != nullptr) { + dynsub->updateParam(); + dynsub = dynsub->next(); + } + + // Check for any newly-enabled subscriptions + updateDynamicSubscriptions(); +} diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp new file mode 100644 index 000000000000..1e25a65d529e --- /dev/null +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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 SubscriptionManager.hpp + * + * Manages the UAVCAN subscriptions + * + * @author Peter van der Perk + */ + +#pragma once + +#ifndef CONFIG_CYPHAL_ESC_SUBSCRIBER +#define CONFIG_CYPHAL_ESC_SUBSCRIBER 0 +#endif + +#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 +#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0 +#endif + +#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 +#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 0 +#endif + +#ifndef CONFIG_CYPHAL_BMS_SUBSCRIBER +#define CONFIG_CYPHAL_BMS_SUBSCRIBER 0 +#endif + +#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER +#define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0 +#endif + +/* Preprocessor calculation of Subscribers count */ + +#define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ + CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ + CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ + CONFIG_CYPHAL_BMS_SUBSCRIBER + \ + CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + +#include +#include +#include "Subscribers/DynamicPortSubscriber.hpp" +#include "CanardInterface.hpp" + +#include "ServiceClients/GetInfo.hpp" +#include "ServiceClients/Access.hpp" +#include "ServiceClients/List.hpp" +#include "Subscribers/BaseSubscriber.hpp" +#include "Subscribers/Heartbeat.hpp" +#include "Subscribers/udral/Battery.hpp" +#include "Subscribers/udral/Esc.hpp" +#include "Subscribers/udral/Gnss.hpp" +#include "Subscribers/legacy/LegacyBatteryInfo.hpp" +#include "Subscribers/uORB/uorb_subscriber.hpp" + +typedef struct { + UavcanDynamicPortSubscriber *(*create_sub)(CanardHandle &handle, UavcanParamManager &pmgr) {}; + const char *subject_name; + const uint8_t instance; +} UavcanDynSubBinder; + +class SubscriptionManager +{ +public: + SubscriptionManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {} + ~SubscriptionManager(); + + void subscribe(); + void printInfo(); + void updateParams(); + +private: + void updateDynamicSubscriptions(); + + CanardHandle &_canard_handle; + UavcanParamManager &_param_manager; + UavcanDynamicPortSubscriber *_dynsubscribers {nullptr}; + + UavcanHeartbeatSubscriber _heartbeat_sub {_canard_handle}; + +#if CONFIG_CYPHAL_GETINFO_RESPONDER + // GetInfo response + UavcanGetInfoResponse _getinfo_rsp {_canard_handle}; +#endif + + // Process register requests + UavcanAccessResponse _access_rsp {_canard_handle, _param_manager}; + UavcanListResponse _list_rsp {_canard_handle, _param_manager}; + + const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] { +#if CONFIG_CYPHAL_ESC_SUBSCRIBER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscSubscriber(handle, pmgr, 0); + }, + "esc", + 0 + }, +#endif +#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanGnssSubscriber(handle, pmgr, 0); + }, + "gps", + 0 + }, +#endif +#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 //FIXME decouple handletanceing + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanGnssSubscriber(handle, pmgr, 1); + }, + "gps", + 1 + }, +#endif +#if CONFIG_CYPHAL_BMS_SUBSCRIBER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanBmsSubscriber(handle, pmgr, 0); + }, + "energy_source", + 0 + }, +#endif +#if 0 //Obsolete to be removed + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0); + }, + "legacy_bms", + 0 + }, +#endif +#if CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new uORB_over_UAVCAN_Subscriber(handle, pmgr, ORB_ID(sensor_gps)); + }, + "uorb.sensor_gps", + 0 + }, +#endif + }; +}; diff --git a/src/drivers/cyphal/legacy_data_types b/src/drivers/cyphal/legacy_data_types new file mode 160000 index 000000000000..36a01e428b11 --- /dev/null +++ b/src/drivers/cyphal/legacy_data_types @@ -0,0 +1 @@ +Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e diff --git a/src/drivers/cyphal/libcanard b/src/drivers/cyphal/libcanard new file mode 160000 index 000000000000..db87ea32aa09 --- /dev/null +++ b/src/drivers/cyphal/libcanard @@ -0,0 +1 @@ +Subproject commit db87ea32aa092c48ea103963138b6346dd3e9008 diff --git a/src/drivers/cyphal/module.yaml b/src/drivers/cyphal/module.yaml new file mode 100644 index 000000000000..0b4f60e602dc --- /dev/null +++ b/src/drivers/cyphal/module.yaml @@ -0,0 +1,10 @@ +module_name: UAVCANv1 +actuator_output: + output_groups: + - param_prefix: UCAN1_ESC + channel_label: 'ESC' + standard_params: + min: { min: 0, max: 8191, default: 1 } + max: { min: 0, max: 8191, default: 8191 } + failsafe: { min: 0, max: 8191 } + num_channels: 16 diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.c b/src/drivers/cyphal/o1heap/o1heap.c similarity index 100% rename from src/drivers/uavcan_v1/o1heap/o1heap.c rename to src/drivers/cyphal/o1heap/o1heap.c diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.h b/src/drivers/cyphal/o1heap/o1heap.h similarity index 100% rename from src/drivers/uavcan_v1/o1heap/o1heap.h rename to src/drivers/cyphal/o1heap/o1heap.h diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c new file mode 100644 index 000000000000..52d3db5b353d --- /dev/null +++ b/src/drivers/cyphal/parameters.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Cyphal + * + * 0 - Cyphal disabled. + * 1 - Enables Cyphal + * + * @boolean + * @reboot_required true + * @group Cyphal + */ +PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0); + +/** + * Cyphal Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min -1 + * @max 125 + * @reboot_required true + * @group Cyphal + */ +PARAM_DEFINE_INT32(CYPHAL_ID, 1); + +/** + * UAVCAN/CAN v1 bus bitrate. + * + * @unit bit/s + * @min 20000 + * @max 1000000 + * @reboot_required true + * @group Cyphal + */ +PARAM_DEFINE_INT32(CYPHAL_BAUD, 1000000); + +/* Subscription port ID, -1 will be treated as unset */ + +/** + * ESC 0 subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1); + +/** + * GPS 0 subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1); + +/** + * GPS 1 subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_GPS1_SUB, -1); + +/** + * UDRAL battery energy source subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_BMS_ES_SUB, -1); + +/** + * UDRAL battery status subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_BMS_BS_SUB, -1); + +/** + * UDRAL battery parameters subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_BMS_BP_SUB, -1); + +/** + * Cyphal leagcy battery port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1); + + +/** + * sensor_gps uORB over Cyphal subscription port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1); + + +/** + * sensor_gps uORB over Cyphal publication port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1); + +// Publication Port IDs + +/** + * Cyphal ESC publication port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); + +/** + * Cyphal GPS publication port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1); + +/** + * Cyphal Servo publication port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1); + +/** + * actuator_outputs uORB over Cyphal publication port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1); diff --git a/src/drivers/cyphal/public_regulated_data_types b/src/drivers/cyphal/public_regulated_data_types new file mode 160000 index 000000000000..d0bd6516dac8 --- /dev/null +++ b/src/drivers/cyphal/public_regulated_data_types @@ -0,0 +1 @@ +Subproject commit d0bd6516dac8ff61287fe49a9f2c75e7d4dc1b8e diff --git a/src/drivers/differential_pressure/CMakeLists.txt b/src/drivers/differential_pressure/CMakeLists.txt deleted file mode 100644 index ab6865d6b36f..000000000000 --- a/src/drivers/differential_pressure/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -############################################################################ -# -# Copyright (c) 2017 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_subdirectory(ets) -add_subdirectory(ms4525) -add_subdirectory(ms5525) -add_subdirectory(sdp3x) \ No newline at end of file diff --git a/src/drivers/differential_pressure/Kconfig b/src/drivers/differential_pressure/Kconfig new file mode 100644 index 000000000000..f044be565fe2 --- /dev/null +++ b/src/drivers/differential_pressure/Kconfig @@ -0,0 +1,11 @@ +menu "Differential pressure" + menuconfig COMMON_DIFFERENTIAL_PRESSURE + bool "Common differential pressure module's" + default n + select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO + select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO + select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X + ---help--- + Enable default set of differential pressure drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/differential_pressure/ets/CMakeLists.txt b/src/drivers/differential_pressure/ets/CMakeLists.txt index fe77c1172c28..23dc3f26aab6 100644 --- a/src/drivers/differential_pressure/ets/CMakeLists.txt +++ b/src/drivers/differential_pressure/ets/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -31,11 +31,13 @@ # ############################################################################ px4_add_module( - MODULE drivers__ets_airspeed + MODULE drivers__differential_pressure__ets_airspeed MAIN ets_airspeed COMPILE_FLAGS SRCS - ets_airspeed.cpp + ets_airspeed_main.cpp + ETSAirspeed.cpp + ETSAirspeed.hpp DEPENDS - drivers__airspeed + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ets/ETSAirspeed.cpp b/src/drivers/differential_pressure/ets/ETSAirspeed.cpp new file mode 100644 index 000000000000..93193a2721dd --- /dev/null +++ b/src/drivers/differential_pressure/ets/ETSAirspeed.cpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "ETSAirspeed.hpp" + +ETSAirspeed::ETSAirspeed(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ETSAirspeed::~ETSAirspeed() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int ETSAirspeed::probe() +{ + _retries = 1; + int ret = measure(); + + return ret; +} + +int ETSAirspeed::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +int ETSAirspeed::measure() +{ + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = READ_CMD; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int ETSAirspeed::collect() +{ + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + perf_count(_comms_errors); + return ret; + } + + float diff_press_pa = (float)(val[1] << 8 | val[0]); + + if (diff_press_pa < FLT_EPSILON) { + // a zero value indicates no measurement + // since the noise floor has been arbitrarily killed + // it defeats our stuck sensor detection - the best we + // can do is to output some numerical noise to show + // that we are still correctly sampling. + diff_press_pa = 0.001f * (timestamp_sample & 0x01); + } + + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = NAN; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + + perf_end(_sample_perf); + + return PX4_OK; +} + +void ETSAirspeed::RunImpl() +{ + int ret; + + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + ret = collect(); + + if (OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_interval > CONVERSION_INTERVAL) { + + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(CONVERSION_INTERVAL); +} diff --git a/src/drivers/differential_pressure/ets/ETSAirspeed.hpp b/src/drivers/differential_pressure/ets/ETSAirspeed.hpp new file mode 100644 index 000000000000..7c3281abcc66 --- /dev/null +++ b/src/drivers/differential_pressure/ets/ETSAirspeed.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * Driver for the Eagle Tree Airspeed V3 connected via I2C. + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x75; /* 7-bit address. 8-bit address is 0xEA */ + +/* Register address */ +#define READ_CMD 0x07 /* Read the data */ + +/** + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * You can set this value to 12 if you want a zero reading below 15km/h. + */ +#define MIN_ACCURATE_DIFF_PRES_PA 0 + +/* Measurement rate is 100Hz */ +#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ + +class ETSAirspeed : public device::I2C, public I2CSPIDriver +{ +public: + ETSAirspeed(const I2CSPIDriverConfig &config); + ~ETSAirspeed() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + +private: + int probe() override; + + int measure(); + int collect(); + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; +}; diff --git a/src/drivers/differential_pressure/ets/Kconfig b/src/drivers/differential_pressure/ets/Kconfig new file mode 100644 index 000000000000..e6e094eea4ab --- /dev/null +++ b/src/drivers/differential_pressure/ets/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_ETS + bool "ets" + default n + ---help--- + Enable support for ets \ No newline at end of file diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp deleted file mode 100644 index f4c23624e376..000000000000 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 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 ets_airspeed.cpp - * @author Simon Wilks - * - * Driver for the Eagle Tree Airspeed V3 connected via I2C. - */ - -#include - -#include -#include -#include -#include - -/* I2C bus address */ -#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ - -/* Register address */ -#define READ_CMD 0x07 /* Read the data */ - -/** - * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. - * You can set this value to 12 if you want a zero reading below 15km/h. - */ -#define MIN_ACCURATE_DIFF_PRES_PA 0 - -/* Measurement rate is 100Hz */ -#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ - -class ETSAirspeed : public Airspeed, public I2CSPIDriver -{ -public: - ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS); - - virtual ~ETSAirspeed() = default; - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - void RunImpl(); -protected: - int measure() override; - int collect() override; -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); - -ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) - : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) -{ - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; -} - -int -ETSAirspeed::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - uint8_t cmd = READ_CMD; - ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int -ETSAirspeed::collect() -{ - int ret = -EIO; - - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - - perf_begin(_sample_perf); - - ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) { - perf_count(_comms_errors); - return ret; - } - - float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]); - - differential_pressure_s report{}; - report.timestamp = hrt_absolute_time(); - - if (diff_pres_pa_raw < FLT_EPSILON) { - // a zero value indicates no measurement - // since the noise floor has been arbitrarily killed - // it defeats our stuck sensor detection - the best we - // can do is to output some numerical noise to show - // that we are still correctly sampling. - diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01); - } - - // The raw value still should be compensated for the known offset - diff_pres_pa_raw -= _diff_pres_offset; - - report.error_count = perf_event_count(_comms_errors); - - // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = diff_pres_pa_raw; - report.differential_pressure_raw_pa = diff_pres_pa_raw; - report.temperature = -1000.0f; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); - - ret = OK; - - perf_end(_sample_perf); - - return ret; -} - -void -ETSAirspeed::RunImpl() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (OK != ret) { - perf_count(_comms_errors); - /* restart the measurement state machine */ - _collect_phase = false; - _sensor_ok = false; - ScheduleNow(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > CONVERSION_INTERVAL) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (OK != ret) { - DEVICE_DEBUG("measure error"); - } - - _sensor_ok = (ret == OK); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(CONVERSION_INTERVAL); -} - -I2CSPIDriverBase *ETSAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ETSAirspeed *instance = new ETSAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->ScheduleNow(); - return instance; -} - - -void -ETSAirspeed::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ets_airspeed_main(int argc, char *argv[]) -{ - using ThisDriver = ETSAirspeed; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - - const char *verb = cli.parseDefaultArguments(argc, argv); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3); - - 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/differential_pressure/ets/ets_airspeed_main.cpp b/src/drivers/differential_pressure/ets/ets_airspeed_main.cpp new file mode 100644 index 000000000000..06c5605b1cb1 --- /dev/null +++ b/src/drivers/differential_pressure/ets/ets_airspeed_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "ETSAirspeed.hpp" + +#include +#include + +void ETSAirspeed::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x75); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ets_airspeed_main(int argc, char *argv[]) +{ + using ThisDriver = ETSAirspeed; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ETS3); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ets/parameters.c b/src/drivers/differential_pressure/ets/parameters.c new file mode 100644 index 000000000000..5a0f7728ffbf --- /dev/null +++ b/src/drivers/differential_pressure/ets/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Eagle Tree airspeed sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_ETSASPD, 0); diff --git a/src/drivers/differential_pressure/ms4515/CMakeLists.txt b/src/drivers/differential_pressure/ms4515/CMakeLists.txt new file mode 100644 index 000000000000..e3e1d1ad5587 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +px4_add_module( + MODULE drivers__differential_pressure__ms4515 + MAIN ms4515 + COMPILE_FLAGS + SRCS + ms4515_main.cpp + MS4515.cpp + MS4515.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/ms4515/Kconfig b/src/drivers/differential_pressure/ms4515/Kconfig new file mode 100644 index 000000000000..b344e1d78685 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4515 + bool "ms4515" + default n + ---help--- + Enable support for ms4515 diff --git a/src/drivers/differential_pressure/ms4515/MS4515.cpp b/src/drivers/differential_pressure/ms4515/MS4515.cpp new file mode 100644 index 000000000000..8abac257f766 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/MS4515.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "MS4515.hpp" + +MS4515::MS4515(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +MS4515::~MS4515() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int MS4515::probe() +{ + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, nullptr, 0); + + return ret; +} + +int MS4515::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +int MS4515::measure() +{ + // Send the command to begin a measurement. + uint8_t cmd = 0; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int MS4515::collect() +{ + /* read from the sensor */ + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + uint8_t val[4] {}; + int ret = transfer(nullptr, 0, &val[0], 4); + + if (ret < 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint8_t status = (val[0] & 0xC0) >> 6; + + switch (status) { + case 0: + // Normal Operation. Good Data Packet + break; + + case 1: + // Reserved + return -EAGAIN; + + case 2: + // Stale Data. Data has been fetched since last measurement cycle. + return -EAGAIN; + + case 3: + // Fault Detected + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + /* mask the used bits */ + int16_t dp_raw = (0x3FFF & ((val[0] << 8) + val[1])); + int16_t dT_raw = (0xFFE0 & ((val[2] << 8) + val[3])) >> 5; + + // dT max is almost certainly an invalid reading + if (dT_raw == 2047) { + perf_count(_comms_errors); + return -EAGAIN; + } + + // only publish changes + if ((dp_raw != _dp_raw_prev) || (dT_raw != _dT_raw_prev)) { + + _dp_raw_prev = dp_raw; + _dT_raw_prev = dT_raw; + + float temperature_c = ((200.0f * dT_raw) / 2047) - 50; + + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative + const float P_min = -1.0f; + const float P_max = 1.0f; + const float IN_H20_to_Pa = 248.84f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet + + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); + float diff_press_pa = diff_press_PSI * IN_H20_to_Pa; + + /* + With the above calculation the MS4515 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + */ + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + } + + perf_end(_sample_perf); + + return PX4_OK; +} + +void MS4515::RunImpl() +{ + int ret = PX4_ERROR; + + // collection phase + if (_collect_phase) { + // perform collection + ret = collect(); + + if (OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + // next phase is measurement + _collect_phase = false; + + // is there a collect->measure gap? + if (_measure_interval > CONVERSION_INTERVAL) { + + // schedule a fresh cycle call when we are ready to measure again + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + // next phase is collection + _collect_phase = true; + + // schedule a fresh cycle call when the measurement is done + ScheduleDelayed(CONVERSION_INTERVAL); +} diff --git a/src/drivers/differential_pressure/ms4515/MS4515.hpp b/src/drivers/differential_pressure/ms4515/MS4515.hpp new file mode 100644 index 000000000000..5b9a04811459 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/MS4515.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * + * Driver for the MEAS Spec series MS4515 connected via I2C. + * + * Supported sensors: + * + * - MS4515DO + * + * Interface application notes: + * + * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x46; + +/* Register address */ +#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + +class MS4515 : public device::I2C, public I2CSPIDriver +{ +public: + MS4515(const I2CSPIDriverConfig &config); + ~MS4515() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + +private: + int probe() override; + + int measure(); + int collect(); + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + int16_t _dp_raw_prev{0}; + int16_t _dT_raw_prev{0}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; +}; diff --git a/src/drivers/differential_pressure/ms4515/ms4515_main.cpp b/src/drivers/differential_pressure/ms4515/ms4515_main.cpp new file mode 100644 index 000000000000..a5f22907cb25 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/ms4515_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "MS4515.hpp" + +#include +#include + +void MS4515::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms4515", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x46); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms4515_main(int argc, char *argv[]) +{ + using ThisDriver = MS4515; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4515); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms4515/parameters.c b/src/drivers/differential_pressure/ms4515/parameters.c new file mode 100644 index 000000000000..25337376a3e7 --- /dev/null +++ b/src/drivers/differential_pressure/ms4515/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * TE MS4515 differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS4515, 0); diff --git a/src/drivers/differential_pressure/ms4525/CMakeLists.txt b/src/drivers/differential_pressure/ms4525/CMakeLists.txt deleted file mode 100644 index bac360d01e9d..000000000000 --- a/src/drivers/differential_pressure/ms4525/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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 drivers__ms4525_airspeed - MAIN ms4525_airspeed - COMPILE_FLAGS - SRCS - ms4525_airspeed.cpp - DEPENDS - drivers__airspeed - mathlib - ) diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp deleted file mode 100644 index f5b982f68289..000000000000 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ /dev/null @@ -1,347 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 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 meas_airspeed.cpp - * @author Lorenz Meier - * @author Sarthak Kaingade - * @author Simon Wilks - * @author Thomas Gubler - * - * Driver for the MEAS Spec series connected via I2C. - * - * Supported sensors: - * - * - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf) - * - * Interface application notes: - * - * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) - */ - -#include -#include -#include -#include - -#include - -enum MS_DEVICE_TYPE { - DEVICE_TYPE_MS4515 = 4515, - DEVICE_TYPE_MS4525 = 4525 -}; - -/* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4515DO 0x46 -#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ - -/* Register address */ -#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ - -/* Measurement rate is 100Hz */ -#define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 1.2f -#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ - - -class MEASAirspeed : public Airspeed, public I2CSPIDriver -{ -public: - MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_MS4525DO); - - virtual ~MEASAirspeed() = default; - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - void RunImpl(); - -protected: - - int measure() override; - int collect() override; - - math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ}; -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]); - -MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) - : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) -{ - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; -} - -int -MEASAirspeed::measure() -{ - // Send the command to begin a measurement. - uint8_t cmd = 0; - int ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int -MEASAirspeed::collect() -{ - /* read from the sensor */ - uint8_t val[4] = {0, 0, 0, 0}; - - perf_begin(_sample_perf); - - int ret = transfer(nullptr, 0, &val[0], 4); - - if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - uint8_t status = (val[0] & 0xC0) >> 6; - - switch (status) { - case 0: - // Normal Operation. Good Data Packet - break; - - case 1: - // Reserved - return -EAGAIN; - - case 2: - // Stale Data. Data has been fetched since last measurement cycle. - return -EAGAIN; - - case 3: - // Fault Detected - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - } - - int16_t dp_raw = 0, dT_raw = 0; - dp_raw = (val[0] << 8) + val[1]; - /* mask the used bits */ - dp_raw = 0x3FFF & dp_raw; - dT_raw = (val[2] << 8) + val[3]; - dT_raw = (0xFFE0 & dT_raw) >> 5; - - // dT max is almost certainly an invalid reading - if (dT_raw == 2047) { - perf_count(_comms_errors); - return -EAGAIN; - } - - float temperature = ((200.0f * dT_raw) / 2047) - 50; - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative - const float P_min = -1.0f; - const float P_max = 1.0f; - const float PSI_to_Pa = 6894.757f; - /* - this equation is an inversion of the equation in the - pressure transfer function figure on page 4 of the datasheet - - We negate the result so that positive differential pressures - are generated when the bottom port is used as the static - port on the pitot and top port is used as the dynamic port - */ - float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min); - float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - - /* - With the above calculation the MS4525 sensor will produce a - positive number when the top port is used as a dynamic port - and bottom port is used as the static port - */ - - differential_pressure_s report{}; - - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.temperature = temperature; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); - - ret = OK; - - perf_end(_sample_perf); - - return ret; -} - -void -MEASAirspeed::RunImpl() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (OK != ret) { - /* restart the measurement state machine */ - _collect_phase = false; - _sensor_ok = false; - ScheduleNow(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_interval > CONVERSION_INTERVAL) { - - /* schedule a fresh cycle call when we are ready to measure again */ - ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (OK != ret) { - DEVICE_DEBUG("measure error"); - } - - _sensor_ok = (ret == OK); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(CONVERSION_INTERVAL); -} - -I2CSPIDriverBase *MEASAirspeed::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MEASAirspeed *instance = new MEASAirspeed(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, - cli.i2c_address); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->ScheduleNow(); - return instance; -} - - -void -MEASAirspeed::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAM_STRING('T', "4525", "4525|4515", "Device type", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ms4525_airspeed_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = MEASAirspeed; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - int device_type = DEVICE_TYPE_MS4525; - - while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { - switch (ch) { - case 'T': - device_type = atoi(cli.optarg()); - break; - } - } - - const char *verb = cli.optarg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - if (device_type == DEVICE_TYPE_MS4525) { - cli.i2c_address = I2C_ADDRESS_MS4525DO; - - } else { - cli.i2c_address = I2C_ADDRESS_MS4515DO; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, - DRV_DIFF_PRESS_DEVTYPE_MS4525); - - 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/differential_pressure/ms4525do/CMakeLists.txt b/src/drivers/differential_pressure/ms4525do/CMakeLists.txt new file mode 100644 index 000000000000..49bf83e18117 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +px4_add_module( + MODULE drivers__differential_pressure__ms4525do + MAIN ms4525do + COMPILE_FLAGS + SRCS + ms4525do_main.cpp + MS4525DO.cpp + MS4525DO.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/ms4525do/Kconfig b/src/drivers/differential_pressure/ms4525do/Kconfig new file mode 100644 index 000000000000..1a1635aa03a9 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO + bool "ms4525do" + default n + ---help--- + Enable support for ms4525do diff --git a/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp b/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp new file mode 100644 index 000000000000..3e321a65a375 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/MS4525DO.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 "MS4525DO.hpp" + +using namespace time_literals; + +MS4525DO::MS4525DO(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +MS4525DO::~MS4525DO() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_fault_perf); +} + +int MS4525DO::probe() +{ + _retries = 1; + + for (int i = 0; i < 10; i++) { + // perform 2 x Read_DF3 (Data Fetch 3 Bytes) + // 1st read: require status = Normal Operation. Good Data Packet + // 2nd read: require status = Stale Data, data should match first read + uint8_t data_1[3] {}; + int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1)); + + uint8_t data_2[3] {}; + int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2)); + + if (ret1 == PX4_OK && ret2 == PX4_OK) { + // Status bits + const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6; + const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6; + + if ((status_1 == (uint8_t)STATUS::Normal_Operation) + && (status_2 == (uint8_t)STATUS::Stale_Data) + && (data_1[2] == data_1[2])) { + + _retries = 1; // enable retries during operation + return PX4_OK; + + } else { + PX4_ERR("status: %X status: %X", status_1, status_2); + } + + } else { + px4_usleep(1000); // TODO + } + } + + return PX4_ERROR; +} + +int MS4525DO::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +void MS4525DO::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_fault_perf); +} + +void MS4525DO::RunImpl() +{ + switch (_state) { + case STATE::MEASURE: { + // Send the command to begin a measurement (Read_MR) + uint8_t cmd = ADDR_READ_MR; + + if (transfer(&cmd, 1, nullptr, 0) == OK) { + _timestamp_sample = hrt_absolute_time(); + _state = STATE::READ; + ScheduleDelayed(2_ms); + + } else { + perf_count(_comms_errors); + _state = STATE::MEASURE; + ScheduleDelayed(10_ms); // try again in 10 ms + } + } + + break; + + case STATE::READ: + // perform 2 x Read_DF4 (Data Fetch 4 Bytes) + // 1st read: require status = Normal Operation. Good Data Packet + // 2nd read: require status = Stale Data, data should match first read + perf_begin(_sample_perf); + uint8_t data_1[4] {}; + int ret1 = transfer(nullptr, 0, &data_1[0], sizeof(data_1)); + + uint8_t data_2[4] {}; + int ret2 = transfer(nullptr, 0, &data_2[0], sizeof(data_2)); + perf_end(_sample_perf); + + if (ret1 != PX4_OK || ret2 != PX4_OK) { + perf_count(_comms_errors); + + } else { + // Status bits + const uint8_t status_1 = (data_1[0] & 0b1100'0000) >> 6; + const uint8_t status_2 = (data_2[0] & 0b1100'0000) >> 6; + + const uint8_t bridge_data_1_msb = (data_1[0] & 0b0011'1111); + const uint8_t bridge_data_2_msb = (data_2[0] & 0b0011'1111); + + const uint8_t bridge_data_1_lsb = data_1[1]; + const uint8_t bridge_data_2_lsb = data_2[1]; + + // Bridge Data [13:8] + Bridge Data [7:0] + int16_t bridge_data_1 = (bridge_data_1_msb << 8) + bridge_data_1_lsb; + int16_t bridge_data_2 = (bridge_data_2_msb << 8) + bridge_data_2_lsb; + + // 11-bit temperature data + // Temperature Data [10:3] + Temperature Data [2:0] + int16_t temperature_1 = ((data_1[2] << 8) + (0b1110'0000 & data_1[3])) / (1 << 5); + int16_t temperature_2 = ((data_2[2] << 8) + (0b1110'0000 & data_2[3])) / (1 << 5); + + if ((status_1 == (uint8_t)STATUS::Fault_Detected) || (status_2 == (uint8_t)STATUS::Fault_Detected)) { + // Fault Detected + perf_count(_fault_perf); + + } else if ((status_1 == (uint8_t)STATUS::Normal_Operation) && (status_2 == (uint8_t)STATUS::Stale_Data) + && (bridge_data_1_msb == bridge_data_2_msb) && (temperature_1 == temperature_2)) { + + float temperature_c = ((200.f * temperature_1) / 2047) - 50.f; + + // Output is proportional to the difference between Port 1 and Port 2. Output swings + // positive when Port 1> Port 2. Output is 50% of supply voltage when Port 1=Port 2. + + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative + static constexpr float P_min = -1.f; // -1 PSI + static constexpr float P_max = 1.f; // +1 PSI + + // this equation is an inversion of the equation in the + // pressure transfer function figure on page 4 of the datasheet + + // We negate the result so that positive differential pressures + // are generated when the bottom port is used as the static + // port on the pitot and top port is used as the dynamic port + const float diff_press_PSI = -((bridge_data_1 - 0.1f * 16383.f) * (P_max - P_min) / (0.8f * 16383.f) + P_min); + + static constexpr float PSI_to_Pa = 6894.757f; + float diff_press_pa = diff_press_PSI * PSI_to_Pa; + + if (hrt_elapsed_time(&_timestamp_sample) < 20_ms) { + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = _timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + + _timestamp_sample = 0; + } + + } else { + PX4_DEBUG("status:%X|%X, B:%X|%X, T:%X|%X", status_1, status_2, bridge_data_1, bridge_data_2, temperature_1, + temperature_2); + } + } + + _state = STATE::MEASURE; + ScheduleDelayed(10_ms); + break; + } +} diff --git a/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp new file mode 100644 index 000000000000..b98483d2f141 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/MS4525DO.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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. + * + ****************************************************************************/ + +/** + * + * Driver for the MEAS Spec series MS4525DO connected via I2C. + * + * Supported sensors: + * + * - MS4525DO + * + * Interface application notes: + * + * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x28; + +/* Register address */ +#define ADDR_READ_MR 0x00 + +class MS4525DO : public device::I2C, public I2CSPIDriver +{ +public: + MS4525DO(const I2CSPIDriverConfig &config); + ~MS4525DO() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + int probe() override; + + enum class STATE : uint8_t { + MEASURE, + READ, + } _state{STATE::MEASURE}; + + enum class STATUS : uint8_t { + Normal_Operation = 0b00, // 0: Normal Operation. Good Data Packet + Reserved = 0b01, // 1: Reserved + Stale_Data = 0b10, // 2: Stale Data. Data has been fetched since last measurement cycle. + Fault_Detected = 0b11, // 3: Fault Detected + }; + + hrt_abstime _timestamp_sample{0}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; + perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": fault detected")}; +}; diff --git a/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp b/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp new file mode 100644 index 000000000000..abea2a67f095 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/ms4525do_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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 "MS4525DO.hpp" + +#include +#include + +void MS4525DO::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms4525do", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x28); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms4525do_main(int argc, char *argv[]) +{ + using ThisDriver = MS4525DO; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS4525DO); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms4525do/parameters.c b/src/drivers/differential_pressure/ms4525do/parameters.c new file mode 100644 index 000000000000..86f722ab1904 --- /dev/null +++ b/src/drivers/differential_pressure/ms4525do/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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. + * + ****************************************************************************/ + +/** + * TE MS4525DO differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS4525DO, 0); diff --git a/src/drivers/differential_pressure/ms5525/CMakeLists.txt b/src/drivers/differential_pressure/ms5525/CMakeLists.txt deleted file mode 100644 index 21e4195034f8..000000000000 --- a/src/drivers/differential_pressure/ms5525/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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 drivers__ms5525_airspeed - MAIN ms5525_airspeed - COMPILE_FLAGS - SRCS - MS5525.cpp - MS5525_main.cpp - DEPENDS - drivers__airspeed - mathlib - ) diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp deleted file mode 100644 index aa5e3f6ea5b3..000000000000 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ /dev/null @@ -1,316 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 "MS5525.hpp" - -int -MS5525::measure() -{ - int ret = PX4_ERROR; - - if (_inited) { - // send the command to begin a conversion. - uint8_t cmd = _current_cmd; - ret = transfer(&cmd, 1, nullptr, 0); - - if (ret != PX4_OK) { - perf_count(_comms_errors); - } - - } else { - _inited = init_ms5525(); - - if (_inited) { - ret = PX4_OK; - } - } - - return ret; -} - -bool -MS5525::init_ms5525() -{ - // Step 1 - reset - uint8_t cmd = CMD_RESET; - int ret = transfer(&cmd, 1, nullptr, 0); - - if (ret != PX4_OK) { - perf_count(_comms_errors); - return false; - } - - px4_usleep(3000); - - // Step 2 - read calibration coefficients from prom - - // prom layout - // 0 factory data and the setup - // 1-6 calibration coefficients - // 7 serial code and CRC - uint16_t prom[8]; - - for (uint8_t i = 0; i < 8; i++) { - cmd = CMD_PROM_START + i * 2; - - // request PROM value - ret = transfer(&cmd, 1, nullptr, 0); - - if (ret != PX4_OK) { - perf_count(_comms_errors); - return false; - } - - // read 2 byte value - uint8_t val[2]; - ret = transfer(nullptr, 0, &val[0], 2); - - if (ret == PX4_OK) { - prom[i] = (val[0] << 8) | val[1]; - - } else { - perf_count(_comms_errors); - return false; - } - } - - // Step 3 - check CRC - const uint8_t crc = prom_crc4(prom); - const uint8_t onboard_crc = prom[7] & 0xF; - - if (crc == onboard_crc) { - // store valid calibration coefficients - C1 = prom[1]; - C2 = prom[2]; - C3 = prom[3]; - C4 = prom[4]; - C5 = prom[5]; - C6 = prom[6]; - - Tref = int64_t(C5) * (1UL << Q5); - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS5525; - - return true; - - } else { - PX4_ERR("CRC mismatch"); - return false; - } -} - -uint8_t -MS5525::prom_crc4(uint16_t n_prom[]) const -{ - // see Measurement Specialties AN520 - - // crc remainder - unsigned int n_rem = 0x00; - - // original value of the crc - unsigned int crc_read = n_prom[7]; // save read CRC - n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0 - - // operation is performed on bytes - for (int cnt = 0; cnt < 16; cnt++) { - // choose LSB or MSB - if (cnt % 2 == 1) { - n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF); - - } else { - n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8); - } - - for (uint8_t n_bit = 8; n_bit > 0; n_bit--) { - if (n_rem & (0x8000)) { - n_rem = (n_rem << 1) ^ 0x3000; - - } else { - n_rem = (n_rem << 1); - } - } - } - - n_rem = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code - n_prom[7] = crc_read; // restore the crc_read to its original place - - return (n_rem ^ 0x00); -} - -int -MS5525::collect() -{ - perf_begin(_sample_perf); - - // read ADC - uint8_t cmd = CMD_ADC_READ; - int ret = transfer(&cmd, 1, nullptr, 0); - - if (ret != PX4_OK) { - perf_count(_comms_errors); - return ret; - } - - // read 24 bits from the sensor - uint8_t val[3]; - ret = transfer(nullptr, 0, &val[0], 3); - - if (ret != PX4_OK) { - perf_count(_comms_errors); - return ret; - } - - uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2]; - - // If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output - // result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and - // the final result will be wrong. Conversion sequence sent during the already started conversion process will yield - // incorrect result as well. - if (adc == 0) { - perf_count(_comms_errors); - return EAGAIN; - } - - if (_current_cmd == CMD_CONVERT_PRES) { - D1 = adc; - _pressure_count++; - - if (_pressure_count > 9) { - _pressure_count = 0; - _current_cmd = CMD_CONVERT_TEMP; - } - - } else if (_current_cmd == CMD_CONVERT_TEMP) { - D2 = adc; - _current_cmd = CMD_CONVERT_PRES; - - // only calculate and publish after new pressure readings - return PX4_OK; - } - - // not ready yet - if (D1 == 0 || D2 == 0) { - return EAGAIN; - } - - // Difference between actual and reference temperature - // dT = D2 - Tref - const int64_t dT = D2 - Tref; - - // Measured temperature - // TEMP = 20°C + dT * TEMPSENS - const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6); - - // Offset at actual temperature - // OFF = OFF_T1 + TCO * dT - const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4); - - // Sensitivity at actual temperature - // SENS = SENS_T1 + TCS * dT - const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3); - - // Temperature Compensated Pressure (example 24996 = 2.4996 psi) - // P = D1 * SENS - OFF - const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15); - - const float diff_press_PSI = P * 0.0001f; - - // 1 PSI = 6894.76 Pascals - static constexpr float PSI_to_Pa = 6894.757f; - const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - - const float temperature_c = TEMP * 0.01f; - - differential_pressure_s diff_pressure = { - .timestamp = hrt_absolute_time(), - .error_count = perf_event_count(_comms_errors), - .differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset, - .differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset, - .temperature = temperature_c, - .device_id = _device_id.devid - }; - - _airspeed_pub.publish(diff_pressure); - - ret = OK; - - perf_end(_sample_perf); - - return ret; -} - -void -MS5525::RunImpl() -{ - int ret = PX4_ERROR; - - // collection phase - if (_collect_phase) { - // perform collection - ret = collect(); - - if (OK != ret) { - /* restart the measurement state machine */ - _collect_phase = false; - _sensor_ok = false; - ScheduleNow(); - return; - } - - // next phase is measurement - _collect_phase = false; - - // is there a collect->measure gap? - if (_measure_interval > CONVERSION_INTERVAL) { - - // schedule a fresh cycle call when we are ready to measure again - ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (OK != ret) { - DEVICE_DEBUG("measure error"); - } - - _sensor_ok = (ret == OK); - - // next phase is collection - _collect_phase = true; - - // schedule a fresh cycle call when the measurement is done - ScheduleDelayed(CONVERSION_INTERVAL); -} diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525/MS5525.hpp deleted file mode 100644 index de052041238d..000000000000 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 - -/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ -static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76; - -/* Measurement rate is 100Hz */ -static constexpr unsigned MEAS_RATE = 100; -static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f; -static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */ - -class MS5525 : public Airspeed, public I2CSPIDriver -{ -public: - MS5525(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_MS5525DSO) : - Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) - { - } - - virtual ~MS5525() = default; - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - void RunImpl(); - -private: - - int measure() override; - int collect() override; - - // temperature is read once every 10 cycles - math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ}; - - static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command - static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command - - static constexpr uint8_t CMD_PROM_START = 0xA0; // Prom read command (first) - - // D1 - pressure convert commands - // Convert D1 (OSR=256) 0x40 - // Convert D1 (OSR=512) 0x42 - // Convert D1 (OSR=1024) 0x44 - // Convert D1 (OSR=2048) 0x46 - // Convert D1 (OSR=4096) 0x48 - static constexpr uint8_t CMD_CONVERT_PRES = 0x44; - - // D2 - temperature convert commands - // Convert D2 (OSR=256) 0x50 - // Convert D2 (OSR=512) 0x52 - // Convert D2 (OSR=1024) 0x54 - // Convert D2 (OSR=2048) 0x56 - // Convert D2 (OSR=4096) 0x58 - static constexpr uint8_t CMD_CONVERT_TEMP = 0x54; - - uint8_t _current_cmd{CMD_CONVERT_PRES}; - - unsigned _pressure_count{0}; - - // Qx Coefficients Matrix by Pressure Range - // 5525DSO-pp001DS (Pmin = -1, Pmax = 1) - static constexpr uint8_t Q1 = 15; - static constexpr uint8_t Q2 = 17; - static constexpr uint8_t Q3 = 7; - static constexpr uint8_t Q4 = 5; - static constexpr uint8_t Q5 = 7; - static constexpr uint8_t Q6 = 21; - - // calibration coefficients from prom - uint16_t C1{0}; - uint16_t C2{0}; - uint16_t C3{0}; - uint16_t C4{0}; - uint16_t C5{0}; - uint16_t C6{0}; - - int64_t Tref{0}; - - // last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature) - uint32_t D1{0}; - uint32_t D2{0}; - - bool init_ms5525(); - bool _inited{false}; - - uint8_t prom_crc4(uint16_t n_prom[]) const; - -}; diff --git a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp b/src/drivers/differential_pressure/ms5525/MS5525_main.cpp deleted file mode 100644 index 991736558f14..000000000000 --- a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 "MS5525.hpp" - -extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]); - -I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MS5525 *instance = new MS5525(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->ScheduleNow(); - return instance; -} - - -void -MS5525::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ms5525_airspeed_main(int argc, char *argv[]) -{ - using ThisDriver = MS5525; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - - const char *verb = cli.parseDefaultArguments(argc, argv); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, - DRV_DIFF_PRESS_DEVTYPE_MS5525); - - 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/differential_pressure/ms5525dso/CMakeLists.txt b/src/drivers/differential_pressure/ms5525dso/CMakeLists.txt new file mode 100644 index 000000000000..1cf253b7aa94 --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-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. +# +############################################################################ + +px4_add_module( + MODULE drivers__differential_pressure__ms5525dso + MAIN ms5525dso + COMPILE_FLAGS + SRCS + ms5525dso_main.cpp + MS5525DSO.cpp + MS5525DSO.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/ms5525dso/Kconfig b/src/drivers/differential_pressure/ms5525dso/Kconfig new file mode 100644 index 000000000000..d5e6653f7f8d --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO + bool "ms5525dso" + default n + ---help--- + Enable support for ms5525dso diff --git a/src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp new file mode 100644 index 000000000000..4b6202537721 --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.cpp @@ -0,0 +1,366 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 "MS5525DSO.hpp" + +MS5525DSO::MS5525DSO(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +MS5525DSO::~MS5525DSO() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int MS5525DSO::probe() +{ + _retries = 1; + + return init_ms5525dso() ? PX4_OK : PX4_ERROR; +} + +int MS5525DSO::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +void MS5525DSO::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int MS5525DSO::measure() +{ + int ret = PX4_ERROR; + + if (_inited) { + // send the command to begin a conversion. + uint8_t cmd = _current_cmd; + ret = transfer(&cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + } + + } else { + _inited = init_ms5525dso(); + + if (_inited) { + ret = PX4_OK; + } + } + + return ret; +} + +bool MS5525DSO::init_ms5525dso() +{ + // Step 1 - reset + uint8_t cmd = CMD_RESET; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return false; + } + + px4_usleep(3000); + + // Step 2 - read calibration coefficients from prom + + // prom layout + // 0 factory data and the setup + // 1-6 calibration coefficients + // 7 serial code and CRC + uint16_t prom[8] {}; + bool prom_all_zero = true; + + for (uint8_t i = 0; i < 8; i++) { + cmd = CMD_PROM_START + i * 2; + + // request PROM value + ret = transfer(&cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + return false; + } + + // read 2 byte value + uint8_t val[2]; + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret == PX4_OK) { + prom[i] = (val[0] << 8) | val[1]; + + if (prom[i] != 0) { + prom_all_zero = false; + } + + } else { + perf_count(_comms_errors); + return false; + } + } + + if (prom_all_zero) { + return false; + } + + // Step 3 - check CRC + const uint8_t crc = prom_crc4(prom); + const uint8_t onboard_crc = prom[7] & 0xF; + + if (crc == onboard_crc) { + // store valid calibration coefficients + C1 = prom[1]; + C2 = prom[2]; + C3 = prom[3]; + C4 = prom[4]; + C5 = prom[5]; + C6 = prom[6]; + + Tref = int64_t(C5) * (1UL << Q5); + + return true; + + } else { + PX4_ERR("CRC mismatch"); + } + + return false; +} + +uint8_t MS5525DSO::prom_crc4(uint16_t n_prom[]) const +{ + // see Measurement Specialties AN520 + + // crc remainder + unsigned int n_rem = 0x00; + + // original value of the crc + unsigned int crc_read = n_prom[7]; // save read CRC + n_prom[7] = (0xFF00 & (n_prom[7])); // CRC byte is replaced by 0 + + // operation is performed on bytes + for (int cnt = 0; cnt < 16; cnt++) { + // choose LSB or MSB + if (cnt % 2 == 1) { + n_rem ^= (unsigned short)((n_prom[cnt >> 1]) & 0x00FF); + + } else { + n_rem ^= (unsigned short)(n_prom[cnt >> 1] >> 8); + } + + for (uint8_t n_bit = 8; n_bit > 0; n_bit--) { + if (n_rem & (0x8000)) { + n_rem = (n_rem << 1) ^ 0x3000; + + } else { + n_rem = (n_rem << 1); + } + } + } + + n_rem = (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code + n_prom[7] = crc_read; // restore the crc_read to its original place + + return (n_rem ^ 0x00); +} + +int MS5525DSO::collect() +{ + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + // read ADC + uint8_t cmd = CMD_ADC_READ; + int ret = transfer(&cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + // read 24 bits from the sensor + uint8_t val[3] {}; + ret = transfer(nullptr, 0, &val[0], 3); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint32_t adc = (val[0] << 16) | (val[1] << 8) | val[2]; + + // If the conversion is not executed before the ADC read command, or the ADC read command is repeated, it will give 0 as the output + // result. If the ADC read command is sent during conversion the result will be 0, the conversion will not stop and + // the final result will be wrong. Conversion sequence sent during the already started conversion process will yield + // incorrect result as well. + if (adc == 0) { + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + } + + if (_current_cmd == CMD_CONVERT_PRES) { + D1 = adc; + _pressure_count++; + + if (_pressure_count > 9) { + _pressure_count = 0; + _current_cmd = CMD_CONVERT_TEMP; + } + + } else if (_current_cmd == CMD_CONVERT_TEMP) { + D2 = adc; + _current_cmd = CMD_CONVERT_PRES; + + // only calculate and publish after new pressure readings + return PX4_OK; + } + + // not ready yet + if (D1 == 0 || D2 == 0) { + perf_end(_sample_perf); + return -EAGAIN; + } + + // Difference between actual and reference temperature + // dT = D2 - Tref + const int64_t dT = D2 - Tref; + + // Measured temperature + // TEMP = 20°C + dT * TEMPSENS + const int64_t TEMP = 2000 + (dT * int64_t(C6)) / (1UL << Q6); + + // Offset at actual temperature + // OFF = OFF_T1 + TCO * dT + const int64_t OFF = int64_t(C2) * (1UL << Q2) + (int64_t(C4) * dT) / (1UL << Q4); + + // Sensitivity at actual temperature + // SENS = SENS_T1 + TCS * dT + const int64_t SENS = int64_t(C1) * (1UL << Q1) + (int64_t(C3) * dT) / (1UL << Q3); + + // Temperature Compensated Pressure (example 24996 = 2.4996 psi) + // P = D1 * SENS - OFF + const int64_t P = (D1 * SENS / (1UL << 21) - OFF) / (1UL << 15); + + const float diff_press_PSI = P * 0.0001f; + + // 1 PSI = 6894.76 Pascals + static constexpr float PSI_to_Pa = 6894.757f; + const float diff_press_pa = diff_press_PSI * PSI_to_Pa; + + const float temperature_c = TEMP * 0.01f; + + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + + perf_end(_sample_perf); + + return PX4_OK; +} + +void MS5525DSO::RunImpl() +{ + int ret = PX4_ERROR; + + // collection phase + if (_collect_phase) { + // perform collection + ret = collect(); + + if (PX4_OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + // next phase is measurement + _collect_phase = false; + + // is there a collect->measure gap? + if (_measure_interval > CONVERSION_INTERVAL) { + + // schedule a fresh cycle call when we are ready to measure again + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (PX4_OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + // next phase is collection + _collect_phase = true; + + // schedule a fresh cycle call when the measurement is done + ScheduleDelayed(CONVERSION_INTERVAL); +} diff --git a/src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp new file mode 100644 index 000000000000..c6408566aaac --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/MS5525DSO.hpp @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include + +/* The MS5525DSODSO address is 111011Cx, where C is the complementary value of the pin CSB */ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x76; + +/* Measurement rate is 100Hz */ +static constexpr unsigned MEAS_RATE = 100; +static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */ + +class MS5525DSO : public device::I2C, public I2CSPIDriver +{ +public: + MS5525DSO(const I2CSPIDriverConfig &config); + ~MS5525DSO() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + int probe() override; + + bool init_ms5525dso(); + + uint8_t prom_crc4(uint16_t n_prom[]) const; + + int measure(); + int collect(); + + static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command + static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command + + static constexpr uint8_t CMD_PROM_START = 0xA0; // Prom read command (first) + + // D1 - pressure convert commands + // Convert D1 (OSR=256) 0x40 + // Convert D1 (OSR=512) 0x42 + // Convert D1 (OSR=1024) 0x44 + // Convert D1 (OSR=2048) 0x46 + // Convert D1 (OSR=4096) 0x48 + static constexpr uint8_t CMD_CONVERT_PRES = 0x48; + + // D2 - temperature convert commands + // Convert D2 (OSR=256) 0x50 + // Convert D2 (OSR=512) 0x52 + // Convert D2 (OSR=1024) 0x54 + // Convert D2 (OSR=2048) 0x56 + // Convert D2 (OSR=4096) 0x58 + static constexpr uint8_t CMD_CONVERT_TEMP = 0x58; + + uint8_t _current_cmd{CMD_CONVERT_PRES}; + + unsigned _pressure_count{0}; + + // Qx Coefficients Matrix by Pressure Range + // 5525DSO-pp001DS (Pmin = -1, Pmax = 1) + static constexpr uint8_t Q1 = 15; + static constexpr uint8_t Q2 = 17; + static constexpr uint8_t Q3 = 7; + static constexpr uint8_t Q4 = 5; + static constexpr uint8_t Q5 = 7; + static constexpr uint8_t Q6 = 21; + + // calibration coefficients from prom + uint16_t C1{0}; + uint16_t C2{0}; + uint16_t C3{0}; + uint16_t C4{0}; + uint16_t C5{0}; + uint16_t C6{0}; + + int64_t Tref{0}; + + // last readings for D1 (uncompensated pressure) and D2 (uncompensated temperature) + uint32_t D1{0}; + uint32_t D2{0}; + + bool _inited{false}; + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; +}; diff --git a/src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp b/src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp new file mode 100644 index 000000000000..a6704f21e986 --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/ms5525dso_main.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 "MS5525DSO.hpp" + +#include +#include + +void MS5525DSO::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ms5525dso", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int ms5525dso_main(int argc, char *argv[]) +{ + using ThisDriver = MS5525DSO; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_MS5525DSO); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/ms5525dso/parameters.c b/src/drivers/differential_pressure/ms5525dso/parameters.c new file mode 100644 index 000000000000..a96d82b981a8 --- /dev/null +++ b/src/drivers/differential_pressure/ms5525dso/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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. + * + ****************************************************************************/ + +/** + * TE MS5525DSO differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_MS5525DS, 0); diff --git a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt index 3ba5eba2fe3c..f0e626485ddc 100644 --- a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt +++ b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-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 @@ -30,14 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__sdp3x_airspeed - MAIN sdp3x_airspeed + MODULE drivers__differential_pressure__sdp3x + MAIN sdp3x COMPILE_FLAGS SRCS + sdp3x_main.cpp SDP3X.cpp - SDP3X_main.cpp + SDP3X.hpp DEPENDS - drivers__airspeed - mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/sdp3x/Kconfig b/src/drivers/differential_pressure/sdp3x/Kconfig new file mode 100644 index 000000000000..45314cfe6bd8 --- /dev/null +++ b/src/drivers/differential_pressure/sdp3x/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X + bool "sdp3x" + default n + ---help--- + Enable support for sdp3x \ No newline at end of file diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 4d7a89d9de4f..82a7c4c24d4e 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-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,20 +31,50 @@ * ****************************************************************************/ -/** - * @file SDP3X.hpp - * - * Driver for Sensirion SDP3X Differential Pressure Sensor - * - */ - #include "SDP3X.hpp" using namespace time_literals; -int -SDP3X::probe() +SDP3X::SDP3X(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _keep_retrying(config.keep_running) +{ +} + +SDP3X::~SDP3X() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SDP3X::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + if (ret == PX4_OK) { + ScheduleNow(); + } + + return ret; +} + +void SDP3X::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} + +int SDP3X::probe() { + _retries = 1; bool require_initialization = !init_sdp3x(); if (require_initialization && _keep_retrying) { @@ -63,16 +93,19 @@ int SDP3X::write_command(uint16_t command) return transfer(&cmd[0], 2, nullptr, 0); } -bool -SDP3X::init_sdp3x() +bool SDP3X::init_sdp3x() { return configure() == 0; } -int -SDP3X::configure() +int SDP3X::configure() { - int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); + int ret = write_command(SDP3X_CONT_MODE_STOP); + + if (ret == PX4_OK) { + px4_udelay(500); // SDP3X is unresponsive for 500us after stop continuous measurement command + ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); + } if (ret != PX4_OK) { perf_count(_comms_errors); @@ -86,8 +119,7 @@ SDP3X::configure() return ret; } -int -SDP3X::read_scale() +int SDP3X::read_scale() { // get scale uint8_t val[9]; @@ -109,34 +141,29 @@ SDP3X::read_scale() switch (_scale) { case SDP3X_SCALE_PRESSURE_SDP31: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP31); break; case SDP3X_SCALE_PRESSURE_SDP32: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP32; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP32); break; case SDP3X_SCALE_PRESSURE_SDP33: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP33; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP33); break; } return PX4_OK; } -void SDP3X::start() -{ - // make sure to wait 10ms after configuring the measurement mode - ScheduleDelayed(10_ms); -} - -int -SDP3X::collect() +int SDP3X::collect() { perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + // read 6 bytes from the sensor - uint8_t val[6]; + uint8_t val[6] {}; int ret = transfer(nullptr, 0, &val[0], sizeof(val)); if (ret != PX4_OK) { @@ -153,27 +180,24 @@ SDP3X::collect() int16_t P = (((int16_t)val[0]) << 8) | val[1]; int16_t temp = (((int16_t)val[3]) << 8) | val[4]; - float diff_press_pa_raw = static_cast(P) / static_cast(_scale); + float diff_press_pa = static_cast(P) / static_cast(_scale); float temperature_c = temp / static_cast(SDP3X_SCALE_TEMPERATURE); - differential_pressure_s report{}; - - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.temperature = temperature_c; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = diff_press_pa; + differential_pressure.temperature = temperature_c; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); perf_end(_sample_perf); return ret; } -void -SDP3X::RunImpl() +void SDP3X::RunImpl() { switch (_state) { case State::RequireConfig: @@ -202,7 +226,6 @@ SDP3X::RunImpl() int ret = collect(); if (ret != 0 && ret != -EAGAIN) { - _sensor_ok = false; DEVICE_DEBUG("measure error"); _state = State::RequireConfig; } diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 755d42429c6a..d9c89c5821e6 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-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 @@ -32,7 +32,6 @@ ****************************************************************************/ /** - * @file SDP3X.hpp * * Driver for Sensirion SDP3X Differential Pressure Sensor * @@ -41,21 +40,24 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include #include +#include +#include -#define I2C_ADDRESS_1_SDP3X 0x21 -#define I2C_ADDRESS_2_SDP3X 0x22 -#define I2C_ADDRESS_3_SDP3X 0x23 +#define I2C_ADDRESS_1_SDP3X 0x21 +#define I2C_ADDRESS_2_SDP3X 0x22 +#define I2C_ADDRESS_3_SDP3X 0x23 + +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface #define SDP3X_SCALE_TEMPERATURE 200.0f #define SDP3X_RESET_ADDR 0x00 #define SDP3X_RESET_CMD 0x06 #define SDP3X_CONT_MEAS_AVG_MODE 0x3615 +#define SDP3X_CONT_MODE_STOP 0x3FF9 #define SDP3X_SCALE_PRESSURE_SDP31 60 #define SDP3X_SCALE_PRESSURE_SDP32 240 @@ -63,44 +65,34 @@ // Measurement rate is 20Hz #define SPD3X_MEAS_RATE 100 -#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */ -class SDP3X : public Airspeed, public I2CSPIDriver +class SDP3X : public device::I2C, public I2CSPIDriver { public: - SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X, - bool keep_retrying = false) : - Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), - _keep_retrying{keep_retrying} - { - } - - virtual ~SDP3X() = default; - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + SDP3X(const I2CSPIDriverConfig &config); + ~SDP3X() override; + static void print_usage(); - void RunImpl(); + void RunImpl(); - void start(); + int init() override; + void print_status() override; private: + int probe() override; + enum class State { RequireConfig, Configuring, Running }; - int measure() override { return 0; } - int collect() override; - int probe() override; - int configure(); - int read_scale(); + int collect(); - math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; + int configure(); + int read_scale(); bool init_sdp3x(); @@ -117,4 +109,9 @@ class SDP3X : public Airspeed, public I2CSPIDriver uint16_t _scale{0}; const bool _keep_retrying; State _state{State::RequireConfig}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; }; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp deleted file mode 100644 index c24e5b6bd2df..000000000000 --- a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 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 "SDP3X.hpp" - -extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); - -I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - SDP3X *instance = new SDP3X(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address, - cli.keep_running); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - - -void -SDP3X::print_usage() -{ - PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21); - PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -sdp3x_airspeed_main(int argc, char *argv[]) -{ - using ThisDriver = SDP3X; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; - cli.i2c_address = I2C_ADDRESS_1_SDP3X; - cli.support_keep_running = true; - - const char *verb = cli.parseDefaultArguments(argc, argv); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31); - - 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/differential_pressure/sdp3x/parameters.c b/src/drivers/differential_pressure/sdp3x/parameters.c new file mode 100644 index 000000000000..47f26836ce68 --- /dev/null +++ b/src/drivers/differential_pressure/sdp3x/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Sensirion SDP3X differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SDP3X, 0); diff --git a/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp new file mode 100644 index 000000000000..ddc48d36de91 --- /dev/null +++ b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 "SDP3X.hpp" + +#include +#include + +void SDP3X::print_usage() +{ + PRINT_MODULE_USAGE_NAME("sdp3x", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int sdp3x_main(int argc, char *argv[]) +{ + using ThisDriver = SDP3X; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_1_SDP3X; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/distance_sensor/CMakeLists.txt b/src/drivers/distance_sensor/CMakeLists.txt index fa20cf8fbdc0..e7e3b632f6ca 100644 --- a/src/drivers/distance_sensor/CMakeLists.txt +++ b/src/drivers/distance_sensor/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ +add_subdirectory(broadcom) add_subdirectory(cm8jl65) add_subdirectory(leddar_one) add_subdirectory(ll40ls) @@ -46,3 +47,4 @@ add_subdirectory(tfmini) add_subdirectory(ulanding_radar) add_subdirectory(vl53l0x) add_subdirectory(vl53l1x) +add_subdirectory(gy_us42) diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig new file mode 100644 index 000000000000..3bbc73ee57d6 --- /dev/null +++ b/src/drivers/distance_sensor/Kconfig @@ -0,0 +1,25 @@ +menu "Distance sensors" + menuconfig COMMON_DISTANCE_SENSOR + bool "Common distance sensor's" + default n + select DRIVERS_DISTANCE_SENSOR_CM8JL65 + select DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE + select DRIVERS_DISTANCE_SENSOR_LL40LS + select DRIVERS_DISTANCE_SENSOR_LL40LS_PWM + select DRIVERS_DISTANCE_SENSOR_MAPPYDOT + select DRIVERS_DISTANCE_SENSOR_MB12XX + select DRIVERS_DISTANCE_SENSOR_PGA460 + select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C + select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL + select DRIVERS_DISTANCE_SENSOR_SRF02 + select DRIVERS_DISTANCE_SENSOR_TERARANGER + select DRIVERS_DISTANCE_SENSOR_TFMINI + select DRIVERS_DISTANCE_SENSOR_ULANDING_RADAR + select DRIVERS_DISTANCE_SENSOR_VL53L0X + select DRIVERS_DISTANCE_SENSOR_VL53L1X + select DRIVERS_DISTANCE_SENSOR_GY_US42 + ---help--- + Enable default set of distance sensor drivers + + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/distance_sensor/broadcom/CMakeLists.txt b/src/drivers/distance_sensor/broadcom/CMakeLists.txt new file mode 100644 index 000000000000..e127e2e9465c --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(afbrs50) diff --git a/src/drivers/distance_sensor/broadcom/Kconfig b/src/drivers/distance_sensor/broadcom/Kconfig new file mode 100644 index 000000000000..6353836988b2 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp new file mode 100644 index 000000000000..af1d5a1eb5a7 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -0,0 +1,579 @@ +/**************************************************************************** + * + * 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 Files */ +#include "AFBRS50.hpp" + +#include + +#include +#include + +/*! Define the SPI baud rate (to be used in the SPI module). */ +#define SPI_BAUD_RATE 5000000 + +#define LONG_RANGE_MODE_HZ 25 +#define SHORT_RANGE_MODE_HZ 50 + +#include "s2pi.h" +#include "timer.h" +#include "argus_hal_test.h" + +AFBRS50 *g_dev{nullptr}; + +AFBRS50::AFBRS50(uint8_t device_orientation): + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _px4_rangefinder(0, device_orientation) +{ + device::Device::DeviceId device_id{}; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SPI; + device_id.devid_s.bus = BROADCOM_AFBR_S50_S2PI_SPI_BUS; + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_AFBRS50; + + _px4_rangefinder.set_device_id(device_id.devid); +} + +AFBRS50::~AFBRS50() +{ + stop(); + + perf_free(_sample_perf); +} + +status_t AFBRS50::measurement_ready_callback(status_t status, void *data) +{ + if (!up_interrupt_context()) { + if (status == STATUS_OK) { + if (g_dev) { + g_dev->ProcessMeasurement(data); + } + + } else { + PX4_ERR("Measurement Ready Callback received error!: %i", (int)status); + } + } + + return status; +} + +void AFBRS50::ProcessMeasurement(void *data) +{ + if (data != nullptr) { + perf_count(_sample_perf); + + argus_results_t res{}; + status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data); + + if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) { + uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); + float result_m = static_cast(result_mm) / 1000.f; + int8_t quality = res.Bin.SignalQuality; + + // Signal quality indicates 100% for good signals, 50% and lower for weak signals. + // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. + if (quality == 1) { + quality = 0; + } + + // distance quality check + if (result_m > _max_distance) { + result_m = 0.0; + quality = 0; + } + + _current_distance = result_m; + _current_quality = quality; + _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); + } + } +} + +int AFBRS50::init() +{ + if (_hnd != nullptr) { + // retry + Argus_Deinit(_hnd); + Argus_DestroyHandle(_hnd); + _hnd = nullptr; + } + + _hnd = Argus_CreateHandle(); + + if (_hnd == nullptr) { + PX4_ERR("Handle not initialized"); + return PX4_ERROR; + } + + // Initialize the S2PI hardware required by the API. + S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE); + + status_t status = Argus_Init(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS); + + if (status == STATUS_OK) { + uint32_t id = Argus_GetChipID(_hnd); + uint32_t value = Argus_GetAPIVersion(); + uint8_t a = (value >> 24) & 0xFFU; + uint8_t b = (value >> 16) & 0xFFU; + uint8_t c = value & 0xFFFFU; + PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c); + + argus_module_version_t mv = Argus_GetModuleVersion(_hnd); + + switch (mv) { + case AFBR_S50MV85G_V1: + + // FALLTHROUGH + case AFBR_S50MV85G_V2: + + // FALLTHROUGH + case AFBR_S50MV85G_V3: + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50MV85G\n"); + break; + + case AFBR_S50LV85D_V1: + _min_distance = 0.08f; + _max_distance = 30.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50LV85D (v1)\n"); + break; + + case AFBR_S50MV68B_V1: + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(1.f)); + PX4_INFO_RAW("AFBR-S50MV68B (v1)\n"); + break; + + case AFBR_S50MV85I_V1: + _min_distance = 0.08f; + _max_distance = 5.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50MV85I (v1)\n"); + break; + + case AFBR_S50SV85K_V1: + _min_distance = 0.08f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(4.f)); + PX4_INFO_RAW("AFBR-S50SV85K (v1)\n"); + break; + + default: + break; + } + + if (_testing) { + _state = STATE::TEST; + + } else { + _state = STATE::CONFIGURE; + } + + ScheduleDelayed(_measure_interval); + return PX4_OK; + } + + return PX4_ERROR; +} + +void AFBRS50::Run() +{ + switch (_state) { + case STATE::TEST: { + if (_testing) { + Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd)); + _testing = false; + + } else { + _state = STATE::CONFIGURE; + } + } + break; + + case STATE::CONFIGURE: { + //status_t status = Argus_SetConfigurationFrameTime(_hnd, _measure_interval); + status_t status = set_rate(SHORT_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("CONFIGURE status not okay: %i", (int)status); + _state = STATE::STOP; + ScheduleNow(); + } + + status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + } + + status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + } + + // start in short range mode + _mode = ARGUS_MODE_B; + set_mode(_mode); + + status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback); + + if (status != STATUS_OK) { + PX4_ERR("CONFIGURE status not okay: %i", (int)status); + ScheduleNow(); + + } else { + _state = STATE::COLLECT; + ScheduleDelayed(_measure_interval); + } + } + break; + + case STATE::COLLECT: { + // currently handeled by measurement_ready_callback + + UpdateMode(); + } + break; + + case STATE::STOP: { + Argus_StopMeasurementTimer(_hnd); + Argus_Deinit(_hnd); + Argus_DestroyHandle(_hnd); + } + break; + + default: + break; + } + + // backup schedule + ScheduleDelayed(100_ms); +} + +void AFBRS50::UpdateMode() +{ + // only update mode if _current_distance is a valid measurement + if ((_current_distance > 0) && (_current_quality > 0)) { + + if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) { + // change to long range mode + argus_mode_t mode = ARGUS_MODE_A; + status_t status = set_mode(mode); + + if (status != STATUS_OK) { + PX4_ERR("set_mode status not okay: %i", (int)status); + } + + status = set_rate(LONG_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("set_rate status not okay: %i", (int)status); + } + + status = set_rate(LONG_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("set_rate status not okay: %i", (int)status); + } + + } else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) { + // change to short range mode + argus_mode_t mode = ARGUS_MODE_B; + status_t status = set_mode(mode); + + if (status != STATUS_OK) { + PX4_ERR("set_mode status not okay: %i", (int)status); + } + + status = set_rate(SHORT_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("set_rate status not okay: %i", (int)status); + } + } + + ScheduleDelayed(1000_ms); // don't switch again for at least 1 second + } +} + +void AFBRS50::stop() +{ + _state = STATE::STOP; + ScheduleNow(); +} + +int AFBRS50::test() +{ + _testing = true; + + init(); + + return PX4_OK; +} + +void AFBRS50::print_info() +{ + perf_print_counter(_sample_perf); + get_info(); +} + +status_t AFBRS50::set_mode(argus_mode_t mode) +{ + while (Argus_GetStatus(_hnd) != STATUS_IDLE) { + px4_usleep(1_ms); + } + + status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status); + return status; + } + + argus_mode_t current_mode; + status = Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); + + if (status != STATUS_OK) { + PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status); + return status; + + } else { + _mode = current_mode; + } + + return status; +} + +status_t AFBRS50::set_rate(uint32_t rate_hz) +{ + while (Argus_GetStatus(_hnd) != STATUS_IDLE) { + px4_usleep(1_ms); + } + + status_t status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz)); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationFrameTime status not okay: %i", (int)status); + return status; + } + + uint32_t current_rate; + status = Argus_GetConfigurationFrameTime(_hnd, ¤t_rate); + + if (status != STATUS_OK) { + PX4_ERR("Argus_GetConfigurationFrameTime status not okay: %i", (int)status); + return status; + + } else { + _measure_interval = current_rate; + } + + return status; +} + +void AFBRS50::get_info() +{ + argus_mode_t current_mode; + argus_dfm_mode_t dfm_mode; + Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); + Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode); + + PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance); + PX4_INFO_RAW("mode: %d\n", current_mode); + PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); + PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval)); +} + +namespace afbrs50 +{ + +static int start(const uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + g_dev = new AFBRS50(rotation); + + if (g_dev == nullptr) { + PX4_ERR("object instantiate failed"); + return PX4_ERROR; + } + + // Initialize the sensor. + if (g_dev->init() != PX4_OK) { + PX4_ERR("driver start failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + g_dev->print_info(); + + return PX4_OK; +} + +static int stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_INFO("driver stopped"); + return PX4_OK; +} + +static int test(const uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + g_dev = new AFBRS50(rotation); + + if (g_dev == nullptr) { + PX4_ERR("object instantiate failed"); + return PX4_ERROR; + } + + if (g_dev->test() != PX4_OK) { + PX4_ERR("driver test failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + +static int usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Driver for the Broadcom AFBRS50. + +### Examples + +Attempt to start driver on a specified serial device. +$ afbrs50 start +Stop driver +$ afbrs50 stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("afbrs50", "driver"); + 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", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int afbrs50_main(int argc, char *argv[]) +{ + const char *myoptarg = nullptr; + + int ch = 0; + int myoptind = 1; + + uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + + while ((ch = px4_getopt(argc, argv, "d:r", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'r': + rotation = (uint8_t)atoi(myoptarg); + break; + + default: + PX4_WARN("Unknown option"); + return afbrs50::usage(); + } + } + + if (myoptind >= argc) { + return afbrs50::usage(); + } + + if (!strcmp(argv[myoptind], "start")) { + return afbrs50::start(rotation); + + } else if (!strcmp(argv[myoptind], "status")) { + return afbrs50::status(); + + } else if (!strcmp(argv[myoptind], "stop")) { + return afbrs50::stop(); + + } else if (!strcmp(argv[myoptind], "test")) { + return afbrs50::test(rotation); + + } + + return afbrs50::usage(); +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp new file mode 100644 index 000000000000..f7503b321bfa --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 AFBRS50.hpp + * + * Driver for the Broadcom AFBR-S50 connected via SPI. + * + */ +#pragma once + +#include "argus.h" + +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class AFBRS50 : public px4::ScheduledWorkItem +{ +public: + AFBRS50(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); + ~AFBRS50() override; + + int init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + /**50 + * Stop the automatic measurement state machine. + */ + void stop(); + + int test(); + + bool _testing = false; + +private: + void Run() override; + + void UpdateMode(); + + void ProcessMeasurement(void *data); + + static status_t measurement_ready_callback(status_t status, void *data); + + void get_info(); + status_t set_mode(argus_mode_t mode); + status_t set_rate(uint32_t rate_hz); + + argus_hnd_t *_hnd{nullptr}; + argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range + + enum class STATE : uint8_t { + TEST, + CONFIGURE, + COLLECT, + STOP + } _state{STATE::CONFIGURE}; + + PX4Rangefinder _px4_rangefinder; + + hrt_abstime _measurement_time{0}; + + perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; + + uint32_t _measure_interval{1000000 / 50}; // 50Hz + float _current_distance{0}; + int8_t _current_quality{0}; + const float _short_range_threshold = 4.0; //meters + const float _long_range_threshold = 6.0; //meters + float _max_distance; + float _min_distance; +}; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/irq.h b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/irq.h new file mode 100644 index 000000000000..6992ce34e44e --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/irq.h @@ -0,0 +1,35 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides functionality to globally turn IRQs on/off. + * + * @copyright Copyright c 2016-2019, Avago Technologies GmbH. + * All rights reserved. + * + *****************************************************************************/ + +#ifndef IRQ_H +#define IRQ_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup IRQ IRQ: Global Interrupt Control + * @ingroup driver + * @brief Global IRQ Module + * @details This module provides functionality to globally enable/disable + * interrupts by turning the I-bit in the CPSR on/off. + * @addtogroup IRQ + * @{ + *****************************************************************************/ + +#include "platform/argus_irq.h" + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif /* IRQ_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/s2pi.h b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/s2pi.h new file mode 100644 index 000000000000..747d16f402fa --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/s2pi.h @@ -0,0 +1,107 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required S2PI module. + * + * @copyright Copyright c 2016-2019, Avago Technologies GmbH. + * All rights reserved. + * + *****************************************************************************/ + +#ifndef S2PI_H +#define S2PI_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup SPI SPI: Serial Peripheral Interface + * @ingroup driver + * @brief S2PI: SPI incl. GPIO Hardware Layer Module + * + * @details Provides driver functionality for the S2PI interface module. + * + * The S2PI module consists of a standard SPI interface plus a + * single GPIO interrupt line. Furthermore, the SPI pins are + * accessible via GPIO control to allow a software emulation of + * additional protocols using the same pins. + * + * This module actually implements the #argus_s2pi interface that + * is required for the Argus API. Refer to the module for more + * information. + * + * @addtogroup SPI + * @{ + *****************************************************************************/ + +#include "platform/argus_s2pi.h" + + +/*! Enables the SPI slaves that utilize a GPIO pin for chip select. */ +#define S2PI_GPIO_SLAVES 0 + +/*! The S2PI slaves. */ +enum S2PISlaves { + /*! No SPI slave selected (all pins are disabled w/ high z state). */ + S2PI_NONE = 0, + + /*! The S2PI slave 1 (connected via adapter board). */ + S2PI_S1 = 1, + + /*! The S2PI slave 2 (connected via cable). */ + S2PI_S2 = 2, + +#if S2PI_GPIO_SLAVES + + /*! The S2PI slave 3. */ + S2PI_S3 = 3, + + /*! The S2PI slave 4. */ + S2PI_S4 = 4, + + /*! The experimental S2PI slave 1 w/ GPIO CS + * (connected via adapter board). */ + S2PI_S1_GPIO = 5, + + /*! The experimental S2PI slave 2 w/ GPIO CS + * (connected via cable). */ + S2PI_S2_GPIO = 6, + +#endif + + /*! No SPI slave selected (all pins go to low state). */ + S2PI_PINS_LOW = 0xFFU, +}; + + +/*!*************************************************************************** + * @brief Initializes the S2PI module. + * + * @details Setup the board as a S2PI master, this also sets up up the S2PI pins. + * The SPI interface is initialized with the corresponding default + * SPI slave (i.e. CS and IRQ lines) and the default baud rate. + * + * @param defaultSlave The default SPI slave to be addressed right after + * module initialization. + * @param baudRate_Bps The default SPI baud rate in bauds-per-second. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps); + +/*!*************************************************************************** + * @brief Sets the SPI baud rate in bps. + * @param baudRate_Bps The default SPI baud rate in bauds-per-second. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * - #STATUS_OK on success + * - #ERROR_S2PI_INVALID_BAUD_RATE on invalid baud rate value. + *****************************************************************************/ +status_t S2PI_SetBaudRate(uint32_t baudRate_Bps); + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif // S2PI_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/timer.h b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/timer.h new file mode 100644 index 000000000000..538b8f8c2e4e --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Inc/timer.h @@ -0,0 +1,52 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides driver functionality for PIT (periodic interrupt timer). + * + * @copyright Copyright c 2016-2019, Avago Technologies GmbH. + * All rights reserved. + * + *****************************************************************************/ + +#ifndef TIMER_H +#define TIMER_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup timer Timer Module + * @ingroup driver + * @brief Timer Hardware Driver Module + * @details Provides driver functionality for the timer peripherals. + * This module actually implements the #argus_timer interface that + * is required for the Argus API. It contains two timing + * functionalities: A periodic interrupt/callback timer and + * an lifetime counter. + * + * Note that the current implementation only features a single + * callback timer interval and does not yet support the feature + * of multiple intervalls at a time. + * + * @addtogroup timer + * @{ + *****************************************************************************/ + +/******************************************************************************* + * Include Files + ******************************************************************************/ + +#include "platform/argus_timer.h" + +/*!*************************************************************************** + * @brief Initializes the timer hardware. + *****************************************************************************/ +void Timer_Init(void); + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif /* TIMER_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c new file mode 100644 index 000000000000..833cf9d48655 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/irq.c @@ -0,0 +1,24 @@ + +#include + +static volatile irqstate_t irqstate_flags; + +/*!*************************************************************************** +* @brief Enable IRQ Interrupts +* +* @return - +*****************************************************************************/ +void IRQ_UNLOCK(void) +{ + leave_critical_section(irqstate_flags); +} + +/*!*************************************************************************** +* @brief Disable IRQ Interrupts +* +* @return - +*****************************************************************************/ +void IRQ_LOCK(void) +{ + irqstate_flags = enter_critical_section(); +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c new file mode 100644 index 000000000000..4442be32f01c --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c @@ -0,0 +1,431 @@ + + +#include "irq.h" +#include "s2pi.h" + +#include + +#include + +#include + +#include +#include + +#include + +/*! A structure to hold all internal data required by the S2PI module. */ +typedef struct { + /*! Determines the current driver status. */ + volatile status_t Status; + + /*! Determines the current S2PI slave. */ + volatile s2pi_slave_t Slave; + + /*! A callback function to be called after transfer/run mode is completed. */ + s2pi_callback_t Callback; + + /*! A parameter to be passed to the callback function. */ + void *CallbackData; + + /*! A callback function to be called after external interrupt is triggered. */ + s2pi_irq_callback_t IrqCallback; + + /*! A parameter to be passed to the interrupt callback function. */ + void *IrqCallbackData; + + struct spi_dev_s *spidev; + uint8_t *spi_tx_data; + uint8_t *spi_rx_data; + size_t spi_frame_size; + + /*! The mapping of the GPIO blocks and pins for this device. */ + const uint32_t GPIOs[ S2PI_IRQ + 1 ]; +} +s2pi_handle_t; + +s2pi_handle_t s2pi_ = { .GPIOs = { [ S2PI_CLK ] = BROADCOM_AFBR_S50_S2PI_CLK, + [ S2PI_CS ] = BROADCOM_AFBR_S50_S2PI_CS, + [ S2PI_MOSI ] = BROADCOM_AFBR_S50_S2PI_MOSI, + [ S2PI_MISO ] = BROADCOM_AFBR_S50_S2PI_MISO, + [ S2PI_IRQ ] = BROADCOM_AFBR_S50_S2PI_IRQ + } + }; + +static struct work_s broadcom_s2pi_transfer_work = {}; + +static perf_counter_t s2pi_transfer_perf = NULL; +static perf_counter_t s2pi_transfer_callback_perf = NULL; +static perf_counter_t s2pi_irq_callback_perf = NULL; + +/*!*************************************************************************** +* @brief Initialize the S2PI module. +* @details Setup the board as a S2PI master, this also sets up up the S2PI +* pins. +* The SPI interface is initialized with the corresponding default +* SPI slave (i.e. CS and IRQ lines) and the default baud rate. +* +* @param defaultSlave The default SPI slave to be addressed right after +* module initialization. +* @param baudRate_Bps The default SPI baud rate in bauds-per-second. +* +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ + +static int gpio_falling_edge(int irq, void *context, void *arg) +{ + if (s2pi_.IrqCallback != 0) { + perf_begin(s2pi_irq_callback_perf); + s2pi_.IrqCallback(s2pi_.IrqCallbackData); + perf_end(s2pi_irq_callback_perf); + } + + return 0; +} + +status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps) +{ + px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_CS); + + s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS); + + px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ); + px4_arch_gpiosetevent(BROADCOM_AFBR_S50_S2PI_IRQ, false, true, false, &gpio_falling_edge, NULL); + + s2pi_transfer_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer"); + s2pi_transfer_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer callback"); + s2pi_irq_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback"); + + return S2PI_SetBaudRate(baudRate_Bps); +} + +/*!*************************************************************************** +* @brief Returns the status of the SPI module. +* +* @return Returns the \link #status_t status\endlink: +* - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. +* - #STATUS_BUSY: An SPI transfer is in progress. +* - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. +*****************************************************************************/ +status_t S2PI_GetStatus(void) +{ + return s2pi_.Status; +} + +/*!*************************************************************************** +* @brief Sets the SPI baud rate in bps. +* @param baudRate_Bps The default SPI baud rate in bauds-per-second. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +* - #STATUS_OK on success +* - #ERROR_S2PI_INVALID_BAUD_RATE on invalid baud rate value. +*****************************************************************************/ +status_t S2PI_SetBaudRate(uint32_t baudRate_Bps) +{ + SPI_SETMODE(s2pi_.spidev, SPIDEV_MODE3); + SPI_SETBITS(s2pi_.spidev, 8); + SPI_SETFREQUENCY(s2pi_.spidev, baudRate_Bps); + return STATUS_OK; +} + +/*!***************************************************************************** +* @brief Captures the S2PI pins for GPIO usage. +* @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the +* pins are configured for GPIO operation. The GPIO control must be +* release with the #S2PI_ReleaseGpioControl function in order to +* switch back to ordinary SPI functionality. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_CaptureGpioControl(void) +{ + /* Check if something is ongoing. */ + irqstate_t irqstate_flags = px4_enter_critical_section(); + status_t status = s2pi_.Status; + + if (status != STATUS_IDLE) { + px4_leave_critical_section(irqstate_flags); + return status; + } + + s2pi_.Status = STATUS_S2PI_GPIO_MODE; + px4_leave_critical_section(irqstate_flags); + + // GPIO mode (output push pull) + px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_CLK])); + px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_MISO])); + px4_arch_configgpio(PX4_MAKE_GPIO_OUTPUT_SET(s2pi_.GPIOs[S2PI_MOSI])); + + return STATUS_OK; +} + +/*!***************************************************************************** +* @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. +* @details The GPIO pins are configured for SPI operation and the GPIO mode is +* left. Must be called if the pins are captured for GPIO operation via +* the #S2PI_CaptureGpioControl function. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_ReleaseGpioControl(void) +{ + /* Check if something is ongoing. */ + irqstate_t irqstate_flags = px4_enter_critical_section(); + status_t status = s2pi_.Status; + + if (status != STATUS_S2PI_GPIO_MODE) { + px4_leave_critical_section(irqstate_flags); + return status; + } + + s2pi_.Status = STATUS_IDLE; + px4_leave_critical_section(irqstate_flags); + + // SPI alternate + stm32_configgpio(s2pi_.GPIOs[S2PI_CLK]); + stm32_configgpio(s2pi_.GPIOs[S2PI_MISO]); + stm32_configgpio(s2pi_.GPIOs[S2PI_MOSI]); + + // probably not necessary + stm32_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS); + + return STATUS_OK; +} + +/*!***************************************************************************** +* @brief Writes the output for a specified SPI pin in GPIO mode. +* @details This function writes the value of an SPI pin if the SPI pins are +* captured for GPIO operation via the #S2PI_CaptureGpioControl previously. +* @param slave The specified S2PI slave. +* @param pin The specified S2PI pin. +* @param value The GPIO pin state to write (0 = low, 1 = high). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value) +{ + /* Check if pin is valid. */ + if (pin > S2PI_IRQ || value > 1) { + return ERROR_INVALID_ARGUMENT; + } + + /* Check if in GPIO mode. */ + if (s2pi_.Status != STATUS_S2PI_GPIO_MODE) { + return ERROR_S2PI_INVALID_STATE; + } + + px4_arch_gpiowrite(s2pi_.GPIOs[pin], value); + + return STATUS_OK; +} + +/*!***************************************************************************** +* @brief Reads the input from a specified SPI pin in GPIO mode. +* @details This function reads the value of an SPI pin if the SPI pins are +* captured for GPIO operation via the #S2PI_CaptureGpioControl previously. +* @param slave The specified S2PI slave. +* @param pin The specified S2PI pin. +* @param value The GPIO pin state to read (0 = low, 1 = high). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value) +{ + /* Check if pin is valid. */ + if (pin > S2PI_IRQ || !value) { + return ERROR_INVALID_ARGUMENT; + } + + /* Check if in GPIO mode. */ + if (s2pi_.Status != STATUS_S2PI_GPIO_MODE) { + return ERROR_S2PI_INVALID_STATE; + } + + *value = px4_arch_gpioread(s2pi_.GPIOs[pin]); + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Cycles the chip select line. +* @details In order to cancel the integration on the ASIC, a fast toggling +* of the chip select pin of the corresponding SPI slave is required. +* Therefore, this function toggles the CS from high to low and back. +* The SPI instance for the specified S2PI slave must be idle, +* otherwise the status #STATUS_BUSY is returned. +* @param slave The specified S2PI slave. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_CycleCsPin(s2pi_slave_t slave) +{ + /* Check the driver status. */ + irqstate_t irqstate_flags = px4_enter_critical_section(); + status_t status = s2pi_.Status; + + if (status != STATUS_IDLE) { + px4_leave_critical_section(irqstate_flags); + return status; + } + + s2pi_.Status = STATUS_BUSY; + px4_leave_critical_section(irqstate_flags); + + px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0); + px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1); + + s2pi_.Status = STATUS_IDLE; + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Transfers a single SPI frame asynchronously. +* @details Transfers a single SPI frame in asynchronous manner. The Tx data +* buffer is written to the device via the MOSI line. +* Optionally the data on the MISO line is written to the provided +* Rx data buffer. If null, the read data is dismissed. +* The transfer of a single frame requires to not toggle the chip +* select line to high in between the data frame. +* An optional callback is invoked when the asynchronous transfer +* is finished. Note that the provided buffer must not change while +* the transfer is ongoing. Use the slave parameter to determine +* the corresponding slave via the given chip select line. +* +* @param slave The specified S2PI slave. +* @param txData The 8-bit values to write to the SPI bus MOSI line. +* @param rxData The 8-bit values received from the SPI bus MISO line +* (pass a null pointer if the data don't need to be read). +* @param frameSize The number of 8-bit values to be sent/received. +* @param callback A callback function to be invoked when the transfer is +* finished. Pass a null pointer if no callback is required. +* @param callbackData A pointer to a state that will be passed to the +* callback. Pass a null pointer if not used. +* +* @return Returns the \link #status_t status\endlink: +* - #STATUS_OK: Successfully invoked the transfer. +* - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. +* - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. +* - #STATUS_BUSY: An SPI transfer is already in progress. The +* transfer was not started. +* - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer +* was not started. +*****************************************************************************/ + +static void broadcom_s2pi_transfer_callout(void *arg) +{ + perf_begin(s2pi_transfer_perf); + px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0); + SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size); + s2pi_.Status = STATUS_IDLE; + px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1); + perf_end(s2pi_transfer_perf); + + /* Invoke callback if there is one */ + if (s2pi_.Callback != 0) { + perf_begin(s2pi_transfer_callback_perf); + s2pi_callback_t callback = s2pi_.Callback; + s2pi_.Callback = 0; + callback(STATUS_OK, s2pi_.CallbackData); + perf_end(s2pi_transfer_callback_perf); + } +} + +status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8_t *rxData, size_t frameSize, + s2pi_callback_t callback, void *callbackData) +{ + /* Verify arguments. */ + if (!txData || frameSize == 0 || frameSize >= 0x10000) { + return ERROR_INVALID_ARGUMENT; + } + + /* Check the spi slave.*/ + if (spi_slave != S2PI_S2) { + return ERROR_S2PI_INVALID_SLAVE; + } + + /* Check the driver status, lock if idle. */ + irqstate_t irqstate_flags = px4_enter_critical_section(); + status_t status = s2pi_.Status; + + if (status != STATUS_IDLE) { + px4_leave_critical_section(irqstate_flags); + return status; + } + + s2pi_.Status = STATUS_BUSY; + + /* Set the callback information */ + s2pi_.Callback = callback; + s2pi_.CallbackData = callbackData; + + s2pi_.spi_tx_data = (uint8_t *)txData; + s2pi_.spi_rx_data = rxData; + s2pi_.spi_frame_size = frameSize; + work_queue(HPWORK, &broadcom_s2pi_transfer_work, broadcom_s2pi_transfer_callout, NULL, 0); + + px4_leave_critical_section(irqstate_flags); + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Terminates a currently ongoing asynchronous SPI transfer. +* @details When a callback is set for the current ongoing activity, it is +* invoked with the #ERROR_ABORTED error byte. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t S2PI_Abort(void) +{ + status_t status = s2pi_.Status; + + /* Check if something is ongoing. */ + if (status == STATUS_IDLE) { + return STATUS_OK; + } + + /* Abort SPI transfer. */ + if (status == STATUS_BUSY) { + work_cancel(HPWORK, &broadcom_s2pi_transfer_work); + } + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Set a callback for the GPIO IRQ for a specified S2PI slave. +* +* @param slave The specified S2PI slave. +* @param callback A callback function to be invoked when the specified +* S2PI slave IRQ occurs. Pass a null pointer to disable +* the callback. +* @param callbackData A pointer to a state that will be passed to the +* callback. Pass a null pointer if not used. +* +* @return Returns the \link #status_t status\endlink: +* - #STATUS_OK: Successfully installation of the callback. +* - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. +*****************************************************************************/ +status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData) +{ + s2pi_.IrqCallback = callback; + s2pi_.IrqCallbackData = callbackData; + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Reads the current status of the IRQ pin. +* @details In order to keep a low priority for GPIO IRQs, the state of the +* IRQ pin must be read in order to reliable check for chip timeouts. +* +* The execution of the interrupt service routine for the data-ready +* interrupt from the corresponding GPIO pin might be delayed due to +* priority issues. The delayed execution might disable the timeout +* for the eye-safety checker too late causing false error messages. +* In order to overcome the issue, the state of the IRQ GPIO input +* pin is read before raising a timeout error in order to check if +* the device has already finished but the IRQ is still pending to be +* executed! +* @param slave The specified S2PI slave. +* @return Returns 1U if the IRQ pin is high (IRQ not pending) and 0U if the +* devices pulls the pin to low state (IRQ pending). +*****************************************************************************/ +uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave) +{ + return px4_arch_gpioread(s2pi_.GPIOs[S2PI_IRQ]); +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c new file mode 100644 index 000000000000..8ace56ea92f6 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c @@ -0,0 +1,145 @@ + +#include "timer.h" + +#include + +#include + +#include + +static struct hrt_call broadcom_hrt_call = {}; + +static timer_cb_t timer_callback_ = 0; /*! Callback function for PIT timer */ + +static uint32_t period_us_ = 1000000 / 1000; // 1000Hz + +static bool isInitialized_ = false; + +/*! Storage for the callback parameter */ +static void *callback_param_; + +static void broadcom_hrt_callout(void *arg) +{ + if ((timer_callback_ != 0) && (period_us_ != 0) && (isInitialized_ == true)) { + //timer_callback_(arg); + timer_callback_(callback_param_); + hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); + + } else { + hrt_cancel(&broadcom_hrt_call); + } +} + +/*!*************************************************************************** +* @brief Obtains the lifetime counter value from the timers. +* +* @details The function is required to get the current time relative to any +* point in time, e.g. the startup time. The returned values \p hct and +* \p lct are given in seconds and microseconds respectively. The current +* elapsed time since the reference time is then calculated from: +* +* t_now [usec] = hct * 1000000 usec + lct * 1 usec +* +* @param hct A pointer to the high counter value bits representing current +* time in seconds. +* @param lct A pointer to the low counter value bits representing current +* time in microseconds. Range: 0, .., 999999 usec +* @return - +*****************************************************************************/ + +void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct) +{ + hrt_abstime time = hrt_absolute_time(); + *hct = time / 1000000ULL; + *lct = time - (*hct * 1000000ULL); +} + +/*!*************************************************************************** +* @brief Starts the timer for a specified callback parameter. +* @details Sets the callback interval for the specified parameter and starts +* the timer with a new interval. If there is already an interval with +* the given parameter, the timer is restarted with the given interval. +* Passing a interval of 0 disables the timer. +* @param dt_microseconds The callback interval in microseconds. +* @param param An abstract parameter to be passed to the callback. This is +* also the identifier of the given interval. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ + +status_t Timer_Start(uint32_t period, void *param) +{ + callback_param_ = param; + period_us_ = period; + + if (period != 0) { + hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); + isInitialized_ = true; + + } else { + hrt_cancel(&broadcom_hrt_call); + } + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Stops the timer for a specified callback parameter. +* @details Stops a callback interval for the specified parameter. +* @param param An abstract parameter that identifies the interval to be stopped. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Timer_Stop(void *param) +{ + period_us_ = 0; + callback_param_ = 0; + isInitialized_ = false; + hrt_cancel(&broadcom_hrt_call); + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Sets the timer interval for a specified callback parameter. +* @details Sets the callback interval for the specified parameter and starts +* the timer with a new interval. If there is already an interval with +* the given parameter, the timer is restarted with the given interval. +* If the same time interval as already set is passed, nothing happens. +* Passing a interval of 0 disables the timer. +* @param dt_microseconds The callback interval in microseconds. +* @param param An abstract parameter to be passed to the callback. This is +* also the identifier of the given interval. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Timer_SetInterval(uint32_t dt_microseconds, void *param) +{ + if (dt_microseconds != 0) { + period_us_ = dt_microseconds; + callback_param_ = param; + isInitialized_ = true; + hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); + + } else { + hrt_cancel(&broadcom_hrt_call); + callback_param_ = 0; + period_us_ = 0; + isInitialized_ = false; + } + + return STATUS_OK; +} + +/*!*************************************************************************** +* @brief Installs an periodic timer callback function. +* @details Installs an periodic timer callback function that is invoked whenever +* an interval elapses. The callback is the same for any interval, +* however, the single intervals can be identified by the passed +* parameter. +* Passing a zero-pointer removes and disables the callback. +* @param f The timer callback function. +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ + +status_t Timer_SetCallback(timer_cb_t f) +{ + timer_callback_ = f; + return STATUS_OK; +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/CMakeLists.txt b/src/drivers/distance_sensor/broadcom/afbrs50/CMakeLists.txt new file mode 100644 index 000000000000..80731288814e --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/CMakeLists.txt @@ -0,0 +1,59 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_library(afbrs50_m4_fpu STATIC IMPORTED GLOBAL) +set_property(TARGET afbrs50_m4_fpu PROPERTY IMPORTED_LOCATION ${CMAKE_CURRENT_SOURCE_DIR}/Lib/libafbrs50_m4_fpu.a) + +px4_add_module( + MODULE drivers__distance_sensor__afbrs50 + MAIN afbrs50 + COMPILE_FLAGS + STACK_MAIN + 4096 + INCLUDES + API/Inc + Inc + SRCS + AFBRS50.cpp + AFBRS50.hpp + API/Src/irq.c + API/Src/s2pi.c + API/Src/timer.c + argus_hal_test.c + DEPENDS + px4_work_queue + drivers_rangefinder + afbrs50_m4_fpu + ) + +target_link_libraries(afbrs50_m4_fpu INTERFACE drivers__distance_sensor__afbrs50) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h new file mode 100644 index 000000000000..a93397637000 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h @@ -0,0 +1,1248 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides generic functionality belonging to all + * devices from the AFBR-S50 product family. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_API_H +#define ARGUS_API_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup argusapi AFBR-S50 API + * + * @brief The main module of the API from the AFBR-S50 SDK. + * + * @details General API for the AFBR-S50 time-of-flight sensor device family.\n + * See the \ref getting_started Guide for a detailed description + * on how to use the module/API. + * + * @addtogroup argusapi + * @{ + *****************************************************************************/ + +#include "argus_def.h" +#include "argus_res.h" +#include "argus_pba.h" +#include "argus_dfm.h" +#include "argus_snm.h" +#include "argus_xtalk.h" + +/*! The data structure for the API representing a AFBR-S50 device instance. */ +typedef void argus_hnd_t; + +/*! The S2PI slave identifier. */ +typedef int32_t s2pi_slave_t; + +/*!*************************************************************************** + * @brief Initializes the API modules and the device with default parameters. + * + * @details The function that needs to be called once after power up to + * initialize the modules state (i.e. the corresponding handle) and the + * dedicated Time-of-Flight device. In order to obtain a handle, + * reference the #Argus_CreateHandle method. + * + * Prior to calling the function, the required peripherals (i.e. S2PI, + * GPIO w/ IRQ and Timers) must be initialized and ready to use. + * + * The function executes the following tasks: + * - Initialization of the internal state represented by the handle + * object. + * - Setup the device such that an safe configuration is present in + * the registers. + * - Initialize sub modules such as calibration or measurement modules. + * . + * + * The modules configuration is initialized with reasonable default values. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_Init(argus_hnd_t *hnd, s2pi_slave_t spi_slave); + +/*!*************************************************************************** + * @brief Reinitializes the API modules and the device with default parameters. + * + * @details The function reinitializes the device with default configuration. + * Can be used as reset sequence for the device. See #Argus_Init for + * more information on the initialization. + * + * Note that the #Argus_Init function must be called first! Otherwise, + * the function will return an error if it is called for an yet + * uninitialized device/handle. + * + * @param hnd The API handle; contains all internal states and data. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_Reinit(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Deinitializes the API modules and the device. + * + * @details The function deinitializes the device and clear all internal states. + * Can be used to cleanup before releasing the memory. The device + * can not be used any more and must be initialized again prior to next + * usage. + * + * Note that the #Argus_Init function must be called first! Otherwise, + * the function will return an error if it is called for an yet + * uninitialized device/handle. + * + * @param hnd The API handle; contains all internal states and data. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_Deinit(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Creates a new device data handle object to store all internal states. + * + * @details The function must be called to obtain a new device handle object. + * The handle is basically an abstract object in memory that contains + * all the internal states and settings of the API module. The handle + * is passed to all the API methods in order to address the specified + * device. This allows to use the API with more than a single measurement + * device. + * + * The handler is created by calling the memory allocation method from + * the standard library: @code void * malloc(size_t size) @endcode + * In order to implement an individual memory allocation method, + * define and implement the following weakly binded method and return + * a pointer to the newly allocated memory. * + * + * @code void * Argus_Malloc (size_t size) @endcode + * + * Also see the #Argus_DestroyHandle method for the corresponding + * deallocation of the allocated memory. + * + * @note Although the method is using memory allocated on the heap, it + * is eventually no dynamic memory allocation, since the block of + * memory is kept all the time and no memory blocks are dynamically + * freed and re-allocated. If the usage of heap must be avoided, one + * can always implement its own version of the `Argus_Malloc` function + * to create the memory elsewhere. + * + * @return Returns a pointer to the newly allocated device handler object. + * Returns a null pointer if the allocation failed! + *****************************************************************************/ +argus_hnd_t *Argus_CreateHandle(void); + +/*!*************************************************************************** + * @brief Destroys a given device data handle object. + * + * @details The function can be called to free the previously created device + * data handle object in order to save memory when the device is not + * used any more. + * + * Please refer to the #Argus_CreateHandle method for the corresponding + * allocation of the memory. + * + * The handler is destroyed by freeing the corresponding memory with the + * method from the standard library, @code void free(void * ptr) @endcode. + * In order to implement an individual memory deallocation method, define + * and implement the following weakly binded method and free the memory + * object passed to the method by a pointer. + * + * @code void Argus_Free (void * ptr) @endcode + * + * Also see the #Argus_CreateHandle method for the corresponding + * allocation of the required memory. + * + * @param hnd The device handle object to be deallocated. + *****************************************************************************/ +void Argus_DestroyHandle(argus_hnd_t *hnd); + +/*!************************************************************************** + * Generic API + ****************************************************************************/ + +/*!*************************************************************************** + * @brief Gets the version number of the current API library. + * + * @details The version is compiled of a major (a), minor (b) and bugfix (c) + * number: a.b.c. + * + * The values are encoded into a 32-bit value: + * + * - [ 31 .. 24 ] - Major Version Number + * - [ 23 .. 16 ] - Minor Version Number + * - [ 15 .. 0 ] - Bugfix Version Number + * . + * + * To obtain the parts from the returned uin32_t value: + * + * @code + * uint32_t value = Argus_GetAPIVersion(); + * uint8_t a = (value >> 24) & 0xFFU; + * uint8_t b = (value >> 16) & 0xFFU; + * uint8_t c = value & 0xFFFFU; + * @endcode + * + * @return Returns the current version number. + *****************************************************************************/ +uint32_t Argus_GetAPIVersion(void); + +/*!*************************************************************************** + * @brief Gets the build number of the current API library. + * + * @return Returns the current build number as a C-string. + *****************************************************************************/ +char const *Argus_GetBuildNumber(void); + +/*!*************************************************************************** + * @brief Gets the version/variant of the module. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current module number. + *****************************************************************************/ +argus_module_version_t Argus_GetModuleVersion(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the version number of the chip. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current version number. + *****************************************************************************/ +argus_chip_version_t Argus_GetChipVersion(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the type number of the device laser. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current device laser type number. + *****************************************************************************/ +argus_laser_type_t Argus_GetLaserType(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the unique identification number of the chip. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the unique identification number. + *****************************************************************************/ +uint32_t Argus_GetChipID(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the SPI hardware slave identifier. + * + * @param hnd The API handle; contains all internal states and data. + * @return The SPI hardware slave identifier. + *****************************************************************************/ +s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd); + +/*! @} */ + +/*!************************************************************************** + * Measurement/Device Operation + **************************************************************************** + * @addtogroup argusmeas + * @{ + ****************************************************************************/ + +/*!*************************************************************************** + * @brief Starts the timer based measurement cycle asynchronously. + * + * @details This function starts a timer based measurement cycle asynchronously + * in the background. A periodic timer interrupt triggers the measurement + * frames on the ASIC and the data readout afterwards. + * + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation as soon as possible in order + * to not introduce any unwanted delays to the next measurement frame. + * + * The next measurement frame will be started as soon as the pre- + * conditions are meet. These are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). + * + * Usually, the device idle and data buffer ready conditions are met + * before the timer tick occurs and thus the timer dictates the frame + * rate. + * + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. + * + * The periodic timer interrupts are used to check the measurement status + * for timeouts. An error is invoked when a measurement cycle have not + * finished within the specified time. + * + * Use #Argus_StopMeasurementTimer to stop the measurements. + * + * @note In order to use this function, the periodic interrupt timer module + * (see @ref argus_timer) must be implemented! + * + * @param hnd The API handle; contains all internal states and data. + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink and a pointer to the \link #argus_results_t results + * \endlink structure. If an error occurred, the status differs + * from #STATUS_OK and the second parameter is null. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, argus_callback_t cb); + +/*!*************************************************************************** + * @brief Stops the timer based measurement cycle. + * + * @details This function stops the ongoing timer based measurement cycles + * that have been started using the #Argus_StartMeasurementTimer + * function. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_StopMeasurementTimer(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Triggers a single measurement frame asynchronously. + * + * @details This function immediately triggers a single measurement frame + * asynchronously if all the pre-conditions are met. Otherwise it + * returns with a corresponding status (e.g. #STATUS_BUSY or + * #STATUS_ARGUS_POWERLIMIT). + * + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation task. + * + * The pre-conditions for starting a measurement frame are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). + * + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. + * + * The successful finishing of the measurement frame is not checked + * for timeouts! Instead, the user can call the #Argus_GetStatus() + * function on a regular function to do so. + * + * @param hnd The API handle; contains all internal states and data. + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink and a pointer to the \link #argus_results_t results + * \endlink structure. If an error occurred, the status differs + * from #STATUS_OK and the second parameter is null. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_TriggerMeasurement(argus_hnd_t *hnd, argus_callback_t cb); + +/*!*************************************************************************** + * @brief Stops the currently ongoing measurements and SPI activity immediately. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_Abort(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Checks the state of the device/driver. + * + * @details Returns the current module status or error if any. + * See the following for a list of errors: + * + * Status: + * - Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE). + * - Busy: Device or SPI interface are busy (== #STATUS_BUSY). + * - Initializing: The modules and devices are currently initializing + * (== #STATUS_INITIALIZING). + * . + * + * Error: + * - Not Initialized: The modules (or any submodule) has not been + * initialized yet (== #ERROR_NOT_INITIALIZED). + * - Not Connected: No device has been connected (or connection errors + * have occurred) (== #ERROR_ARGUS_NOT_CONNECTED). + * - Timeout: A previous frame measurement has not finished within a + * specified time (== #ERROR_TIMEOUT). + * . + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetStatus(argus_hnd_t *hnd); + +/*!***************************************************************************** + * @brief Tests the connection to the device by sending a ping message. + * + * @details A ping is transfered to the device in order to check the device and + * SPI connection status. Returns #STATUS_OK on success and + * #ERROR_ARGUS_NOT_CONNECTED elsewise. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + ******************************************************************************/ +status_t Argus_Ping(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Evaluate useful information from the raw measurement data. + * + * @details This function is called with a pointer to the raw results obtained + * from the measurement cycle. It evaluates this data and creates + * useful information from it. Furthermore, calibration is applied to + * the data. Finally, the results are used in order to adapt the device + * configuration to the ambient conditions in order to achieve optimal + * device performance. + * + * Therefore, it consists of the following sub-functions: + * - Apply pre-calibration: Applies calibration steps before evaluating + * the data, i.e. calculations that are to the integration results + * directly. + * - Evaluate data: Calculates measurement parameters such as range, + * amplitude or ambient light intensity, depending on the configurations. + * - Apply post-calibration: Applies calibrations after evaluation of + * measurement data, i.e. calibrations applied to the calculated + * values such as range. + * - Dynamic Configuration Adaption: checks if the configuration needs + * to be adjusted before the next measurement cycle in order to + * achieve optimum performance. Note that the configuration might not + * applied directly but before the next measurement starts. This is + * due to the fact that the device could be busy measuring already + * the next frame and thus no SPI activity is allowed. + * . + * However, if the device is idle, the configuration will be written + * immediately. + * + * @param hnd The API handle; contains all internal states and data. + * @param res A pointer to the results structure that will be populated + * with evaluated data. + * @param raw The pointer to the raw data that has been obtained by the + * measurement finished callback. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_EvaluateData(argus_hnd_t *hnd, argus_results_t *res, void *raw); + +/*!*************************************************************************** + * @brief Executes a crosstalk calibration measurement. + * + * @details This function immediately triggers a crosstalk vector calibration + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. + * + * In order to perform a crosstalk calibration, the reflection of the + * transmitted signal must be kept from the receiver side, by either + * covering the TX completely (or RX respectively) or by setting up + * an absorbing target at far distance. + * + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationCrosstalkVectorTable function. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd, argus_mode_t mode); + + +/*!*************************************************************************** + * @brief Executes a relative range offset calibration measurement. + * + * @details This function immediately triggers a relative range offset calibration + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. + * + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. + * + * \code + * AFBR-S50 ToF Sensor + * #| + * #| | + * #|-----+ | + * #| Rx | | + * Reference #|----++ | Calibration + * Plane #| Tx | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode + * + * There are two options to run the offset calibration: relative and + * absolute. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . + * + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd, + argus_mode_t mode); + +/*!*************************************************************************** + * @brief Executes an absolute range offset calibration measurement. + * + * @details This function immediately triggers an absolute range offset calibration + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. + * + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. + * + * \code + * AFBR-S50 ToF Sensor + * #| + * #| | + * #|-----+ | + * #| Rx | | + * Reference #|----++ | Calibration + * Plane #| Tx | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode + * + * There are two options to run the offset calibration: relative and + * absolute. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . + * + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param targetRange The absolute range between the reference plane and the + * calibration target in meter an Q9.22 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence(argus_hnd_t *hnd, + argus_mode_t mode, + q9_22_t targetRange); + +/*! @} */ + +/*!************************************************************************** + * Configuration API + **************************************************************************** + * @addtogroup arguscfg + * @{ + ****************************************************************************/ + +/*!*************************************************************************** + * @brief Sets the measurement mode to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationMeasurementMode(argus_hnd_t *hnd, + argus_mode_t value); + +/*!*************************************************************************** + * @brief Gets the measurement mode from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationMeasurementMode(argus_hnd_t *hnd, + argus_mode_t *value); + +/*!*************************************************************************** + * @brief Sets the frame time to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The measurement frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t value); + +/*!*************************************************************************** + * @brief Gets the frame time from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t *value); + +/*!*************************************************************************** + * @brief Sets the smart power save enabled flag to a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, + argus_mode_t mode, + bool value); + +/*!*************************************************************************** + * @brief Gets the smart power save enabled flag from a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, + argus_mode_t mode, + bool *value); + +/*!*************************************************************************** + * @brief Sets the Dual Frequency Mode (DFM) to a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationDFMMode(argus_hnd_t *hnd, + argus_mode_t mode, + argus_dfm_mode_t value); + + +/*!*************************************************************************** + * @brief Gets the Dual Frequency Mode (DFM) from a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationDFMMode(argus_hnd_t *hnd, + argus_mode_t mode, + argus_dfm_mode_t *value); + +/*!*************************************************************************** + * @brief Sets the Shot Noise Monitor (SNM) mode to a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, + argus_mode_t mode, + argus_snm_mode_t value); + +/*!*************************************************************************** + * @brief Gets the Shot Noise Montor (SNM) mode from a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, + argus_mode_t mode, + argus_snm_mode_t *value); +#if 0 +///*!*************************************************************************** +// * @brief Sets the Crosstalk Monitor (XTM) mode to a specified device. +// * @param hnd The API handle; contains all internal states and data. +// * @param mode The targeted measurement mode. +// * @param value The new XTM mode value (true: enabled; false: disabled). +// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +// *****************************************************************************/ +//status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, +// argus_mode_t mode, +// bool value); +// +///*!*************************************************************************** +// * @brief Gets the Crosstalk Monitor (XTM) mode from a specified device. +// * @param hnd The API handle; contains all internal states and data. +// * @param mode The targeted measurement mode. +// * @param value The current XTM mode value (true: enabled; false: disabled). +// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +// *****************************************************************************/ +//status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, +// argus_mode_t mode, +// bool * value); +#endif + +/*!*************************************************************************** + * @brief Sets the full DCA module configuration to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new DCA configuration set. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationDynamicAdaption(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cfg_dca_t const *value); + +/*!*************************************************************************** + * @brief Gets the # from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current DCA configuration set value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationDynamicAdaption(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cfg_dca_t *value); +/*!*************************************************************************** + * @brief Sets the pixel binning configuration parameters to a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetConfigurationPixelBinning(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cfg_pba_t const *value); + +/*!*************************************************************************** + * @brief Gets the pixel binning configuration parameters from a specified device. + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationPixelBinning(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cfg_pba_t *value); + +/*!*************************************************************************** + * @brief Gets the current unambiguous range in mm. + * @param hnd The API handle; contains all internal states and data. + * @param range_mm The returned range in mm. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, + uint32_t *range_mm); + +/*! @} */ + +/*!************************************************************************** + * Calibration API + **************************************************************************** + * @addtogroup arguscal + * @{ + ****************************************************************************/ + +/*!*************************************************************************** + * @brief Sets the global range offset value to a specified device. + * + * @details The global range offset is subtracted from the raw range values. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, + argus_mode_t mode, + q0_15_t value); + +/*!*************************************************************************** + * @brief Gets the global range offset value from a specified device. + * + * @details The global range offset is subtracted from the raw range values. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, + argus_mode_t mode, + q0_15_t *value); + +/*!*************************************************************************** + * @brief Sets the relative pixel offset table to a specified device. + * + * @details The relative pixel offset values are subtracted from the raw range + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! + * + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t. + * + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . + * + * Its recommended to use the built-in pixel offset calibration + * sequence (see #Argus_ExecuteRelativeRangeOffsetCalibrationSequence) + * to determine the offset table for the current device. + * + * If a constant offset table for all device needs to be incorporated + * into the sources, the #Argus_GetExternalPixelRangeOffsets_Callback + * should be used. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, + q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + + +/*!*************************************************************************** + * @brief Gets the relative pixel offset table from a specified device. + * + * @details The relative pixel offset values are subtracted from the raw range + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! + * + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t. + * + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, + q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + + +/*!*************************************************************************** + * @brief Gets the relative pixel offset table from a specified device. + * + * @details The relative pixel offset values are subtracted from the raw range + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! + * + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t. + * + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * + * The total offset table consists of the custom pixel offset values + * (set via #Argus_SetCalibrationPixelRangeOffsets) and the internal, + * factory calibrated device specific offset values. + * This is informational only! + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current total relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationTotalPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, + q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + + +/*!*************************************************************************** + * @brief Resets the relative pixel offset values for the specified device to + * the factory calibrated default values. + * + * @details The relative pixel offset values are subtracted from the raw range + * values for each individual pixel. Note that a global range offset + * is applied additionally. + * + * The factory defaults are device specific values. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ResetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode); + +/*!*************************************************************************** + * @brief A callback that returns the external pixel range offsets. + * + * @details The function needs to be implemented by the host application in + * order to set the external pixel range offsets values upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero offset table, assuming there + * is no (additional) external pixel range offset values. + * + * If defined in user code, the function must fill all offset values + * in the provided \par offsets parameter with external range offset + * values. + * The values can be obtained by the calibration routine. + * + * Example usage: + * + * @code + * status_t Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], + * argus_mode_t mode) + * { + * (void) mode; // Ignore mode; use same values for all modes. + * memset(offsets, 0, sizeof(q0_15_t) * ARGUS_PIXELS); + * + * // Set offset values in meter and Q0.15 format. + * offsets[0][0].dS = -16384; offsets[0][0].dC = -32768; + * offsets[0][1].dS = -32768; offsets[0][1].dC = 0; + * offsets[0][2].dS = 16384; offsets[0][2].dC = -16384; + * // etc. + * } + * @endcode + * + * @param offsets The pixel range offsets in meter and Q0.15 format; to be + * filled with data. + * @param mode Determines the current measurement mode; can be ignored if + * only a single measurement mode is utilized. + *****************************************************************************/ +void Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], + argus_mode_t mode); + +/*!*************************************************************************** + * @brief Sets the sample time for the range offset calibration sequence. + * + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the offset data.\n + * Units: msec. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t value); + +/*!*************************************************************************** + * @brief Gets the sample time for the range offset calibration sequence. + * + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the ooffset data.\n + * Units: msec. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value); + +/*!*************************************************************************** + * @brief Sets the pixel-to-pixel crosstalk compensation parameters to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cal_p2pxtalk_t const *value); + +/*!*************************************************************************** + * @brief Gets the pixel-to-pixel crosstalk compensation parameters from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, + argus_mode_t mode, + argus_cal_p2pxtalk_t *value); + + +/*!*************************************************************************** + * @brief Sets the custom crosstalk vector table to a specified device. + * + * @details The crosstalk vectors are subtracted from the raw sampling data + * in the data evaluation phase. + * + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. + * + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . + * + * Its recommended to use the built-in crosstalk calibration sequence + * (see #Argus_ExecuteXtalkCalibrationSequence) to determine the + * crosstalk vector table. + * + * If a constant table for all device needs to be incorporated into + * the sources, the #Argus_GetExternalCrosstalkVectorTable_Callback + * should be used. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The new crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, + argus_mode_t mode, + xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + +/*!*************************************************************************** + * @brief Gets the custom crosstalk vector table from a specified device. + * + * @details The crosstalk vectors are subtracted from the raw sampling data + * in the data evaluation phase. + * + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. + * + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, + argus_mode_t mode, + xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + +/*!*************************************************************************** + * @brief Gets the factory calibrated default crosstalk vector table for the + * specified device. + * + * @details The crosstalk vectors are subtracted from the raw sampling data + * in the data evaluation phase. + * + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. + * + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . + * + * The total vector table consists of the custom crosstalk vector + * table (set via #Argus_SetCalibrationCrosstalkVectorTable) and + * an internal, factory calibrated device specific vector table. + * This is informational only! + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @param value The current total crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationTotalCrosstalkVectorTable(argus_hnd_t *hnd, + argus_mode_t mode, + xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + +/*!*************************************************************************** + * @brief Resets the crosstalk vector table for the specified device to the + * factory calibrated default values. + * + * @details The crosstalk vectors are subtracted from the raw sampling data + * in the data evaluation phase. + * * + * The factory defaults are device specific calibrated values. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The targeted measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, + argus_mode_t mode); + +/*!*************************************************************************** + * @brief Sets the sample time for the crosstalk calibration sequence. + * + * @details Sets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, + uint16_t value); + +/*!*************************************************************************** + * @brief Gets the sample time for the crosstalk calibration sequence. + * + * @details Gets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, + uint16_t *value); + +/*!*************************************************************************** + * @brief Sets the max. amplitude threshold for the crosstalk calibration sequence. + * + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence maximum amplitude + * threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, + uq12_4_t value); + +/*!*************************************************************************** + * @brief Gets the max. amplitude threshold for the crosstalk calibration sequence. + * + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current max. amplitude threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, + uq12_4_t *value); + +/*!*************************************************************************** + * @brief Sets the sample count for the substrate voltage calibration sequence. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new substrate voltage calibration sequence sample count. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_SetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, + uint16_t value); + +/*!*************************************************************************** + * @brief Gets the sample count for the substrate voltage calibration sequence. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current substrate voltage calibration sequence sample count. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, + uint16_t *value); + +/*!*************************************************************************** + * @brief A callback that returns the external crosstalk vector table. + * + * @details The function needs to be implemented by the host application in + * order to set the external crosstalk vector table upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero vector table, assuming there + * is no (additional) external crosstalk. + * + * If defined in user code, the function must fill all vector values + * in the provided \par xtalk parameter with external crosstalk values. + * The values can be obtained by the calibration routine. + * + * Example usage: + * + * @code + * status_t Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], + * argus_mode_t mode) + * { + * (void) mode; // Ignore mode; use same values for all modes. + * memset(&xtalk, 0, sizeof(xtalk)); + * + * // Set crosstalk vectors in Q11.4 format. + * // Note on dual-frequency frame index: 0 = A-Frame; 1 = B-Frame + * xtalk[0][0][0].dS = -9; xtalk[0][0][0].dC = -11; + * xtalk[0][0][1].dS = -13; xtalk[0][0][1].dC = -16; + * xtalk[0][0][2].dS = 6; xtalk[0][0][2].dC = -18; + * // etc. + * } + * @endcode + * + * @param xtalk The crosstalk vector array; to be filled with data. + * @param mode Determines the current measurement mode; can be ignored if + * only a single measurement mode is utilized. + *****************************************************************************/ +void Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t + xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], + argus_mode_t mode); + + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif /* ARGUS_API_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h new file mode 100644 index 000000000000..f23d1176484e --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h @@ -0,0 +1,497 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dynamic configuration adaption (DCA) setup parameters + * and data structure. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_DCA_H +#define ARGUS_DCA_H + +/*!*************************************************************************** + * @defgroup argusdca Dynamic Configuration Adaption + * @ingroup argusapi + * + * @brief Dynamic Configuration Adaption (DCA) parameter definitions and API functions. + * + * @details The DCA contains an algorithms that detect ambient conditions + * and adopt the device configuration to the changing parameters + * dynamically while operating the sensor. This is achieved by + * rating the currently received signal quality and changing the + * device configuration accordingly to the gathered information + * from the current measurement frame results before the next + * integration cycle starts. + * + * The DCA consists of the following features: + * - Static or Dynamic mode. The first is utilizing the nominal + * values while the latter is dynamically adopting between min. + * and max. value and starting from the nominal values. + * - Analog Integration Depth Adaption (from multiple patterns down to single pulses) + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption (w/ ambient light rejection) + * - ADC Sensitivity (i.e. ADC Range) Adaption + * - Power Saving Ratio (to decrease the average output power and thus the current consumption) + * - All that features are heeding the Laser Safety limits. + * . + * + * @addtogroup argusdca + * @{ + *****************************************************************************/ + +#include "argus_def.h" + + + +/*! The minimum amplitude threshold value. */ +#define ARGUS_CFG_DCA_ATH_MIN (1U << 6U) + +/*! The maximum amplitude threshold value. */ +#define ARGUS_CFG_DCA_ATH_MAX (0xFFFFU) + + +/*! The minimum saturated pixel threshold value. */ +#define ARGUS_CFG_DCA_PXTH_MIN (1U) + +/*! The maximum saturated pixel threshold value. */ +#define ARGUS_CFG_DCA_PXTH_MAX (33U) + + +/*! The maximum analog integration depth in UQ10.6 format, + * i.e. the maximum pattern count per sample. */ +#define ARGUS_CFG_DCA_DEPTH_MAX ((uq10_6_t)(ADS_SEQCT_N_MASK << (6U - ADS_SEQCT_N_SHIFT))) + +/*! The minimum analog integration depth in UQ10.6 format, + * i.e. the minimum pattern count per sample. */ +#define ARGUS_CFG_DCA_DEPTH_MIN ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble + + +/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ +#define ARGUS_CFG_DCA_POWER_MAX_LSB (ADS_LASET_VCSEL_HC1_MASK >> ADS_LASET_VCSEL_HC1_SHIFT) + +/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ +#define ARGUS_CFG_DCA_POWER_MIN_LSB (1) + +/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ +#define ARGUS_CFG_DCA_POWER_MAX (ADS0032_HIGH_CURRENT_LSB2MA(ARGUS_CFG_DCA_POWER_MAX_LSB + 1)) + +/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ +#define ARGUS_CFG_DCA_POWER_MIN (1) + + +/*! The dynamic configuration algorithm Pixel Input Gain stage count. */ +#define ARGUS_DCA_GAIN_STAGE_COUNT (4U) + +/*! The dynamic configuration algorithm state mask for the Pixel Input Gain stage. */ +#define ARGUS_STATE_DCA_GAIN_MASK (0x03U) + +/*! The dynamic configuration algorithm state mask for the Pixel Input Gain stage. */ +#define ARGUS_STATE_DCA_GAIN_SHIFT (14U) + +/*! Getter for the dynamic configuration algorithm Pixel Input Gain stage. */ +#define ARGUS_STATE_DCA_GAIN_GET(state) \ + (((state) >> ARGUS_STATE_DCA_GAIN_SHIFT) & ARGUS_STATE_DCA_GAIN_MASK) + + +/*! The dynamic configuration algorithm Optical Output Power stage count. */ +#define ARGUS_DCA_POWER_STAGE_COUNT (2U) + +/*! The dynamic configuration algorithm state mask for the Optical Output Power stage. */ +#define ARGUS_STATE_DCA_POWER_MASK (0x01U) + +/*! The dynamic configuration algorithm state mask for the Optical Output Power stage. */ +#define ARGUS_STATE_DCA_POWER_SHIFT (13U) + +/*! Getter for the dynamic configuration algorithm Optical Output Power stage. */ +#define ARGUS_STATE_DCA_POWER_GET(state) \ + (((state) >> ARGUS_STATE_DCA_POWER_SHIFT) & ARGUS_STATE_DCA_POWER_MASK) + + + + +/*!*************************************************************************** + * @brief The dynamic configuration algorithm enable flags. + *****************************************************************************/ +typedef enum { + /*! @internal + * + * DCA is disabled and will be completely skipped. + * + * @note This state is for internal/debugging usage only as it also + * disables laser safety checks of the device configuration. + * An error will occur when used with the API.*/ + DCA_ENABLE_OFF = 0, + + /*! DCA is enabled and will dynamically adjust the device configuration. */ + DCA_ENABLE_DYNAMIC = 1, + + /*! DCA is enabled and will apply the static (nominal) values to the device. */ + DCA_ENABLE_STATIC = -1, + +} argus_dca_enable_t; + +/*!*************************************************************************** + * @brief The DCA amplitude evaluation method. + *****************************************************************************/ +typedef enum { + /*! Evaluate the DCA amplitude as the maximum of all valid amplitudes. */ + DCA_AMPLITUDE_MAX = 1U, + + /*! Evaluate the DCA amplitude as the average of all valid amplitudes. */ + DCA_AMPLITUDE_AVG = 2U, + +} argus_dca_amplitude_mode_t; + +/*!*************************************************************************** + * @brief The dynamic configuration algorithm Optical Output Power stages enumerator. + *****************************************************************************/ +typedef enum { + /*! Use low output power stage. */ + DCA_POWER_LOW = 0, + + /*! Use high output power stage. */ + DCA_POWER_HIGH = 1, + + /*! Use low and high output power stages automatically. */ + DCA_POWER_AUTO = 2 + +} argus_dca_power_t; + +/*!*************************************************************************** + * @brief The dynamic configuration algorithm Pixel Input Gain stages enumerator. + *****************************************************************************/ +typedef enum { + /*! Low gain stage. */ + DCA_GAIN_LOW = 0, + + /*! Medium low gain stage. */ + DCA_GAIN_MEDIUM_LOW = 1, + + /*! Medium high gain stage. */ + DCA_GAIN_MEDIUM_HIGH = 2, + + /*! High gain stage. */ + DCA_GAIN_HIGH = 3 + +} argus_dca_gain_t; + + +/*!*************************************************************************** + * @brief State flags for the current frame. + * @details State flags determine the current state of the measurement frame: + * - [0]: #ARGUS_STATE_MEASUREMENT_MODE + * - [1]: #ARGUS_STATE_DUAL_FREQ_MODE + * - [2]: #ARGUS_STATE_MEASUREMENT_FREQ + * - [3]: #ARGUS_STATE_DEBUG_MODE + * - [4]: #ARGUS_STATE_WEAK_SIGNAL + * - [5]: #ARGUS_STATE_BGL_WARNING + * - [6]: #ARGUS_STATE_BGL_ERROR + * - [7]: #ARGUS_STATE_PLL_LOCKED + * - [8]: #ARGUS_STATE_LASER_WARNING + * - [9]: #ARGUS_STATE_LASER_ERROR + * - [10]: #ARGUS_STATE_HAS_DATA + * - [11]: #ARGUS_STATE_HAS_AUX_DATA + * - [12]: #ARGUS_STATE_DCA_MAX + * - [13]: DCA Power Stage + * - [14-15]: DCA Gain Stages + * . + *****************************************************************************/ +typedef enum { + /*! No state flag set. */ + ARGUS_STATE_NONE = 0, + + /*! 0x0001: Measurement Mode. + * - 0: Mode A: Long Range / Medium Precision + * - 1: Mode B: Short Range / High Precision */ + ARGUS_STATE_MEASUREMENT_MODE = 1U << 0U, + + /*! 0x0002: Dual Frequency Mode Enabled. + * - 0: Disabled: measurements with base frequency, + * - 1: Enabled: measurement with detuned frequency. */ + ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U, + + /*! 0x0004: Measurement Frequency for Dual Frequency Mode + * (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set). + * - 0: A-Frame w/ detuned frequency, + * - 1: B-Frame w/ detuned frequency */ + ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U, + + /*! 0x0008: Debug Mode. If set, the range value of erroneous pixels + * are not cleared or reset. + * - 0: Disabled (default). + * - 1: Enabled. */ + ARGUS_STATE_DEBUG_MODE = 1U << 3U, + + /*! 0x0010: Weak Signal Flag. + * Set whenever the Pixel Binning Algorithm is detecting a + * weak signal, i.e. if the amplitude dies not reach its + * (absolute) threshold. If the Golden Pixel is enabled, + * this also indicates that the Pixel Binning Algorithm + * falls back to the Golden Pixel. + * - 0: Normal Signal. + * - 1: Weak Signal or Golden Pixel Mode. */ + ARGUS_STATE_WEAK_SIGNAL = 1U << 4U, + + /*! 0x0020: Background Light Warning Flag. + * Set whenever the background light is very high and the + * measurement data might be unreliable. + * - 0: No Warning: Background Light is within valid range. + * - 1: Warning: Background Light is very high. */ + ARGUS_STATE_BGL_WARNING = 1U << 5U, + + /*! 0x0040: Background Light Error Flag. + * Set whenever the background light is too high and the + * measurement data is unreliable or invalid. + * - 0: No Error: Background Light is within valid range. + * - 1: Error: Background Light is too high. */ + ARGUS_STATE_BGL_ERROR = 1U << 6U, + + /*! 0x0080: PLL_LOCKED bit. + * - 0: PLL not locked at start of integration. + * - 1: PLL locked at start of integration. */ + ARGUS_STATE_PLL_LOCKED = 1U << 7U, + + /*! 0x0100: Laser Failure Warning Flag. + * Set whenever the an invalid system condition is detected. + * (i.e. DCA at max state but no amplitude on any (incl. reference) + * pixel, not amplitude but any saturated pixel). + * - 0: No Warning: Laser is operating properly. + * - 1: Warning: Invalid laser conditions detected. If the invalid + * condition stays, a laser malfunction error is raised. */ + ARGUS_STATE_LASER_WARNING = 1U << 8U, + + /*! 0x0200: Laser Failure Error Flag. + * Set whenever a laser malfunction error is raised and the + * system is put into a safe state. + * - 0: No Error: Laser is operating properly. + * - 1: Error: Invalid laser conditions are detected for a certain + * soak time and the system is put into a safe state. */ + ARGUS_STATE_LASER_ERROR = 1U << 9U, + + /*! 0x0400: Set if current frame has distance measurement data available. + * - 0: No measurement data available, all values are 0 or stalled. + * - 1: Measurement data is available and correctly evaluated. */ + ARGUS_STATE_HAS_DATA = 1U << 10U, + + /*! 0x0800: Set if current frame has auxiliary measurement data available. + * - 0: No auxiliary data available, all values are 0 or stalled. + * - 1: Auxiliary data is available and correctly evaluated. */ + ARGUS_STATE_HAS_AUX_DATA = 1U << 11U, + + /*! 0x1000: DCA Maximum State Flag. + * Set whenever the DCA has extended all its parameters to their + * maximum values and can not increase the integration energy any + * further. + * - 0: DCA has not yet reached its maximum state. + * - 1: DCA has reached its maximum state and can not increase any further. */ + ARGUS_STATE_DCA_MAX = 1U << 12U, + + /*! 0x2000: DCA is in high Optical Output Power stage. */ + ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT, + + /*! DCA is in low Pixel Input Gain stage. */ + ARGUS_STATE_DCA_GAIN_LOW = DCA_GAIN_LOW << ARGUS_STATE_DCA_GAIN_SHIFT, + + /*! 0x4000: DCA is in medium-low Pixel Input Gain stage. */ + ARGUS_STATE_DCA_GAIN_MED_LOW = DCA_GAIN_MEDIUM_LOW << ARGUS_STATE_DCA_GAIN_SHIFT, + + /*! 0x8000: DCA is in medium-high Pixel Input Gain stage. */ + ARGUS_STATE_DCA_GAIN_MED_HIGH = DCA_GAIN_MEDIUM_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT, + + /*! 0xC000: DCA is in high Pixel Input Gain stage. */ + ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT, + +} argus_state_t; + +/*!*************************************************************************** + * @brief Dynamic Configuration Adaption (DCA) Parameters. + * @details DCA contains: + * - Static or dynamic mode. The first is utilizing the nominal values + * while the latter is dynamically adopting between min. and max. + * value and starting form the nominal values. + * - Analog Integration Depth Adaption down to single pulses. + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption + * - Digital Integration Depth Adaption + * - Dynamic Global Phase Shift Injection. + * - All that features are heeding the Laser Safety limits. + * . + *****************************************************************************/ +typedef struct { + /*! Enables the automatic configuration adaption features. + * Enables the dynamic part if #DCA_ENABLE_DYNAMIC and the static only if + * #DCA_ENABLE_STATIC. */ + argus_dca_enable_t Enabled; + + /*! The threshold value of saturated pixels that causes a linear reduction + * of the integration energy, i.e. if the number of saturated pixels are + * larger or equal to this value, the integration energy will be reduced + * by a single step (one pattern if the current integration depth is > 1, + * one pulse if the current integration depth is <= 1 or one power LSB for + * the optical power range). + * + * Valid values: 1, ..., 33; (use 33 to disable the linear decrease) + * Note that the linear value must be smaller or equal to the exponential + * value. To sum up, it must hold: + * 1 <= SatPxThLin <= SatPxThExp <= SatPxThRst <= 33 */ + uint8_t SatPxThLin; + + /*! The threshold number of saturated pixels that causes a exponential + * reduction of the integration energy, i.e. if the number of saturated + * pixels is larger or equal to this value, the integration energy will be + * halved. + * + * Valid values: 1, ..., 33; (use 33 to disable the exponential decrease) + * Note that the exponential value must be between the linear and reset + * values. To sum up, it must hold: + * 1 <= SatPxThLin <= SatPxThExp <= SatPxThRst <= 33 */ + uint8_t SatPxThExp; + + /*! The threshold number of saturated pixels that causes a sudden reset of + * the integration energy to the minimal value, i.e. if the number of + * saturated pixels are larger or equal to this value, the integration + * energy will suddenly be reset to the minimum values. The gain setting + * will stay at the mid value and a decrease happens after the next step + * if still required. + * + * Valid values: 1, ..., 33; (use 33 to disable the sudden reset) + * Note that the reset value must be larger or equal to the exponential + * value. To sum up, it must hold: + * 1 <= SatPxThLin <= SatPxThExp <= SatPxThRst <= 33 */ + uint8_t SatPxThRst; + + /*! The DCA amplitude to be targeted from the lower regime. If the DCA + * amplitude lower than the target value, a linear increase of integration + * energy will happen in order to optimize for best performance. + * + * Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX + * Note further that the following condition must hold: + * 'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */ + uq12_4_t Atarget; + + /*! The low threshold value for the DCA amplitude. If the DCA amplitude + * falls below this value, the integration depth will be increases. + * + * Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX + * Note further that the following condition must hold: + * 'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */ + uq12_4_t AthLow; + + /*! The high threshold value for the DCA amplitude. If the DCA amplitude + * exceeds this value, the integration depth will be decreases. Note that + * also saturated pixels will cause a decrease of the integration depth. + * + * Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX + * Note further that the following condition must hold: + * 'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */ + uq12_4_t AthHigh; + + /*! The DCA amplitude calculation algorithm. Either maximum + * (#DCA_AMPLITUDE_MAX) or average (#DCA_AMPLITUDE_AVG) amplitude can be + * selected. */ + argus_dca_amplitude_mode_t AmplitudeMode; + + /*! The power stage selector. + * Selects the used power stages, i.e. LOW, HIGH or AUTO (LOW+HIGH). */ + argus_dca_power_t Power; + + /*! The nominal analog integration depth in UQ10.6 format, + * i.e. the nominal pattern count per sample. + * + * Valid values: #ARGUS_CFG_DCA_DEPTH_MIN, ... #ARGUS_CFG_DCA_DEPTH_MAX + * Note further that the following condition must hold: + * 'MIN' <= DepthLow <= DepthNom <= DepthHigh <= 'MAX' */ + uq10_6_t DepthNom; + + /*! The minimum analog integration depth in UQ10.6 format, + * i.e. the minimum pattern count per sample. + * + * Valid values: #ARGUS_CFG_DCA_DEPTH_MIN, ... #ARGUS_CFG_DCA_DEPTH_MAX + * Note further that the following condition must hold: + * 'MIN' <= DepthLow <= DepthNom <= DepthHigh <= 'MAX' */ + uq10_6_t DepthMin; + + /*! The maximum analog integration depth in UQ10.6 format, + * i.e. the maximum pattern count per sample. + * + * Valid values: #ARGUS_CFG_DCA_DEPTH_MIN, ... #ARGUS_CFG_DCA_DEPTH_MAX + * Note further that the following condition must hold: + * 'MIN' <= DepthMin <= DepthNom <= DepthMax <= 'MAX' */ + uq10_6_t DepthMax; + + /*! The nominal pixel gain setting, i.e. the setting for + * nominal/default gain stage. + * + * Valid values: 0,..,3: #DCA_GAIN_LOW, ... #DCA_GAIN_HIGH + * Note further that the following condition must hold: + * 'MIN' <= GainMin <= GainNom <= GainMax <= 'MAX' */ + argus_dca_gain_t GainNom; + + /*! The minimal pixel gain setting, i.e. the setting for + * minimum gain stage. + * + * Valid values: 0,..,3: #DCA_GAIN_LOW, ... #DCA_GAIN_HIGH + * Note further that the following condition must hold: + * 'MIN' <= GainMin <= GainNom <= GainMax <= 'MAX' */ + argus_dca_gain_t GainMin; + + /*! The maximum pixel gain setting, i.e. the setting for + * maximum gain stage. + * + * Valid values: 0,..,3: #DCA_GAIN_LOW, ... #DCA_GAIN_HIGH + * Note further that the following condition must hold: + * 'MIN' <= GainMin <= GainNom <= GainMax <= 'MAX' */ + argus_dca_gain_t GainMax; + + /*! Power Saving Ratio value. + * + * Determines the percentage of the full available frame time that is not + * exploited for digital integration. Thus the device is idle within the + * specified portion of the frame time and does consume less energy. + * + * Note that the laser safety might already limit the maximum integration + * depth and the power saving ratio might not take effect for all ambient + * situations. Thus the Power Saving Ratio is to be understood as a minimum + * percentage where the device is idle per frame. + * + * The value is a UQ0.8 format that ranges from 0.0 (=0x00) to 0.996 (=0xFF), + * where 0 means no power saving (i.e. feature disabled) and 0xFF determines + * maximum power saving, i.e. the digital integration depth is limited to a + * single sample. + * + * Range: 0x00, .., 0xFF; set 0 to disable. */ + uq0_8_t PowerSavingRatio; + +} argus_cfg_dca_t; + +/*! @} */ +#endif /* ARGUS_DCA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h new file mode 100644 index 000000000000..c639922a7d19 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h @@ -0,0 +1,214 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 hardware API. + * @details This file provides generic definitions belonging to all + * devices from the AFBR-S50 product family. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_DEF_H +#define ARGUS_DEF_H + +/*!*************************************************************************** + * Include files + *****************************************************************************/ +#include "argus_status.h" +#include "argus_version.h" +#include "utility/fp_def.h" +#include "utility/time.h" +#include +#include +#include +#include +#include + +/*!*************************************************************************** + * @addtogroup argusapi + * @{ + *****************************************************************************/ + +/*!*************************************************************************** + * @brief Maximum number of phases per measurement cycle. + * @details The actual phase number is defined in the register configuration. + * However the software does only support a fixed value of 4 yet. + *****************************************************************************/ +#define ARGUS_PHASECOUNT 4U + +/*!*************************************************************************** + * @brief The device pixel field size in x direction (long edge). + *****************************************************************************/ +#define ARGUS_PIXELS_X 8U + +/*!*************************************************************************** + * @brief The device pixel field size in y direction (short edge). + *****************************************************************************/ +#define ARGUS_PIXELS_Y 4U + +/*!*************************************************************************** + * @brief The total device pixel count. + *****************************************************************************/ +#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) + +/*!*************************************************************************** + * @brief The AFBR-S50 module types. + *****************************************************************************/ +typedef enum { + /*! No device connected or not recognized. */ + MODULE_NONE = 0, + + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 1 - legacy version! */ + AFBR_S50MV85G_V1 = 1, + + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 2 - legacy version! */ + AFBR_S50MV85G_V2 = 2, + + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 7 - current version! */ + AFBR_S50MV85G_V3 = 7, + + /*! AFBR-S50LV85D: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * long range 1D applications. + * Version 1 - current version! */ + AFBR_S50LV85D_V1 = 3, + + /*! AFBR-S50MV68B: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and red, 680 nm, laser source for + * medium range 1D applications. + * Version 1 - current version! */ + AFBR_S50MV68B_V1 = 4, + + /*! AFBR-S50MV85I: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 1 - current version! */ + AFBR_S50MV85I_V1 = 5, + + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * short range 3D applications. + * Version 1 - current version! */ + AFBR_S50SV85K_V1 = 6, + + + /*! Reserved for future extensions. */ + Reserved = 0x3F + +} argus_module_version_t; + +/*!*************************************************************************** + * @brief The AFBR-S50 laser configurations. + *****************************************************************************/ +typedef enum { + /*! No laser connected. */ + LASER_NONE = 0, + + /*! 850nm Infra-Red VCSEL v1 */ + LASER_H_V1 = 1, + + /*! 850nm Infra-Red VCSEL v2 */ + LASER_H_V2 = 2, + + /*! 680nm Red VCSEL v1 */ + LASER_R_V1 = 3, + + /*! 850nm Infra-Red VCSEL v2 /w extended mode. */ + LASER_H_V2X = 4, + +} argus_laser_type_t; + +/*!*************************************************************************** + * @brief The AFBR-S50 chip versions. + *****************************************************************************/ +typedef enum { + /*! No device connected or not recognized. */ + ADS0032_NONE = 0, + + /*! ADS0032 v1a */ + ADS0032_V1A = 1, + + /*! ADS0032 v1b */ + ADS0032_V1B = 2, + + /*! ADS0032 v1c */ + ADS0032_V1C = 3, + + /*! ADS0032 v1d */ + ADS0032_V1D = 4, + + /*! ADS0032 v1e */ + ADS0032_V1E = 5, + +} argus_chip_version_t; + +/*!*************************************************************************** + * @brief The number of measurement modes with distinct configuration and + * calibration records. + *****************************************************************************/ +#define ARGUS_MODE_COUNT (2) + +/*!*************************************************************************** + * @brief The measurement modes. + *****************************************************************************/ +typedef enum { + /*! Measurement Mode A: Long Range Mode. */ + ARGUS_MODE_A = 1, + + /*! Measurement Mode B: Short Range Mode. */ + ARGUS_MODE_B = 2, + +} argus_mode_t; + +/*!*************************************************************************** + * @brief Generic API callback function. + * @details Invoked by the API. The content of the abstract data pointer + * depends upon the context. + * @param status The module status that caused the callback. #STATUS_OK if + * everything was as expected. + * @param data An abstract pointer to an user defined data. This will usually + * be passed to the function that also takes the callback as an + * parameter. Otherwise it has a special meaning such as + * configuration or calibration data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +typedef status_t (*argus_callback_t)(status_t status, void *data); + +/*! @} */ +#endif /* ARGUS_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h new file mode 100644 index 000000000000..b2517182f871 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h @@ -0,0 +1,81 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dual frequency mode (DFM) setup parameters. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_DFM_H +#define ARGUS_DFM_H + +/*!*************************************************************************** + * @defgroup argusdfm Dual Frequency Mode + * @ingroup argusapi + * + * @brief Dual Frequency Mode (DFM) parameter definitions and API functions. + * + * @details The DFM is an algorithm to extend the unambiguous range of the + * sensor by utilizing two detuned measurement frequencies. + * + * The AFBR-S50 API provides three measurement modes: + * - 1X: Single Frequency Measurement + * - 4X: Dual Frequency Measurement w/ 4 times the unambiguous + * range of the Single Frequency Measurement + * - 8X: Dual Frequency Measurement w/ 8 times the unambiguous + * range of the Single Frequency Measurement + * + * @addtogroup argusdfm + * @{ + *****************************************************************************/ + +/*! The Dual Frequency Mode frequency count. */ +#define ARGUS_DFM_FRAME_COUNT (2U) + +/*! The Dual Frequency Mode measurement modes count. Excluding the disabled mode. */ +#define ARGUS_DFM_MODE_COUNT (2U) // expect off-mode! + +/*! The Dual Frequency Mode measurement modes enumeration. */ +typedef enum { + /*! Single Frequency Measurement Mode (w/ 1x Unambiguous Range). */ + DFM_MODE_OFF = 0U, + + /*! 4X Dual Frequency Measurement Mode (w/ 4x Unambiguous Range). */ + DFM_MODE_4X = 1U, + + /*! 8X Dual Frequency Measurement Mode (w/ 8x Unambiguous Range). */ + DFM_MODE_8X = 2U, + +} argus_dfm_mode_t; + + +/*! @} */ +#endif /* ARGUS_DFM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h new file mode 100644 index 000000000000..64588d25f154 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h @@ -0,0 +1,253 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines macros to work with pixel and ADC channel masks. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_MAP_H +#define ARGUS_MAP_H + +/*!*************************************************************************** + * @defgroup argusmap ADC Channel Mapping + * @ingroup argusres + * + * @brief Pixel ADC Channel Mapping + * + * @details The ADC Channels of each pixel or auxiliary channel on the device + * are numbered in a way that is convenient on the chip architecture. + * The macros in this module are defined in order to map between the + * chip internal channel number (ch) to the two-dimensional + * x-y-indices or one-dimensional n-index representation. + * + * @addtogroup argusmap + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" +#include "utility/int_math.h" + + + + +/*!***************************************************************************** + * @brief Macro to determine the pixel ADC channel number from the x-z-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The ADC channel number of the pixel. + ******************************************************************************/ +#define PIXEL_XY2CH(x, y) ((((y) << 3U) & 0x10U) | (((x) ^ 0x07U) << 1U) | ((y) & 0x01U)) + +/*!***************************************************************************** + * @brief Macro to determine the pixel x-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The x-index of the pixel. + ******************************************************************************/ +#define PIXEL_CH2X(c) ((((c) >> 1U) ^ 0x07U) & 0x07U) + +/*!***************************************************************************** + * @brief Macro to determine the pixel y-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The y-index of the pixel. + ******************************************************************************/ +#define PIXEL_CH2Y(c) ((((c) >> 3U) & 0x02U) | ((c) & 0x01U)) + + +/*!***************************************************************************** + * @brief Macro to determine the n-index from the x-y-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The n-index of the pixel. + ******************************************************************************/ +#define PIXEL_XY2N(x, y) (((x) << 2U) | (y)) + +/*!***************************************************************************** + * @brief Macro to determine the pixel x-index from the n-index. + * @param n The n-index of the pixel. + * @return The x-index number of the pixel. + ******************************************************************************/ +#define PIXEL_N2X(n) ((n) >> 2U) + +/*!***************************************************************************** + * @brief Macro to determine the pixel y-index from the n-index. + * @param n The n-index of the pixel. + * @return The y-index number of the pixel. + ******************************************************************************/ +#define PIXEL_N2Y(n) ((n) & 0x03U) + + +/*!***************************************************************************** + * @brief Macro to determine the pixel n-index from the ADC channel number. + * @param n The n-index of the pixel. + * @return The ADC channel number of the pixel. + ******************************************************************************/ +#define PIXEL_N2CH(n) ((((n) << 3U) & 0x10U) | ((((n) >> 1U) ^ 0x0EU) & 0x0EU) | ((n) & 0x01U)) + +/*!***************************************************************************** + * @brief Macro to determine the pixel + * @param c The ADC channel number of the pixel. + * @return The n-index of the pixel. + ******************************************************************************/ +#define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U)) + + +/*!***************************************************************************** + * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel. + * @return True if the pixel (n) is enabled. + ******************************************************************************/ +#define PIXELN_ISENABLED(msk, n) (((msk) >> (n)) & 0x01U) + +/*!***************************************************************************** + * @brief Macro to enable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to enable. + ******************************************************************************/ +#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n))) + +/*!***************************************************************************** + * @brief Macro disable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to disable. + ******************************************************************************/ +#define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n)))) + + +/*!***************************************************************************** + * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The ADC channel number of the pixel. + * @return True if the specified pixel ADC channel is enabled. + ******************************************************************************/ +#define PIXELCH_ISENABLED(msk, c) (PIXELN_ISENABLED(msk, PIXEL_CH2N(c))) + +/*!***************************************************************************** + * @brief Macro to enable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to enable. + ******************************************************************************/ +#define PIXELCH_ENABLE(msk, c) (PIXELN_ENABLE(msk, PIXEL_CH2N(c))) + +/*!***************************************************************************** + * @brief Macro to disable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to disable. + ******************************************************************************/ +#define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c))) + + +/*!***************************************************************************** + * @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel. + * @param y y-index of the pixel. + * @return True if the pixel (x,y) is enabled. + ******************************************************************************/ +#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y))) + +/*!***************************************************************************** + * @brief Macro to enable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to enable. + * @param y y-index of the pixel to enable. + ******************************************************************************/ +#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y))) + +/*!***************************************************************************** + * @brief Macro disable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to disable. + * @param y y-index of the pixel to disable. + ******************************************************************************/ +#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y))) + + +/*!***************************************************************************** + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel is enabled. + ******************************************************************************/ +#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U) + +/*!***************************************************************************** + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to enabled. + ******************************************************************************/ +#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U))) + +/*!***************************************************************************** + * @brief Macro to determine if an ADC channel is disabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to disable. + ******************************************************************************/ +#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U)))) + + +/*!***************************************************************************** + * @brief Macro to determine the number of enabled pixel/channels in a mask + * via a popcount algorithm. + * @param pxmsk 32-bit pixel mask + * @return The count of enabled pixel channels. + ******************************************************************************/ +#define PIXEL_COUNT(pxmsk) popcount(pxmsk) + +/*!***************************************************************************** + * @brief Macro to determine the number of enabled channels via a popcount + * algorithm. + * @param pxmsk 32-bit pixel mask + * @param chmsk 32-bit channel mask + * @return The count of enabled ADC channels. + ******************************************************************************/ +#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk)) + +/*!***************************************************************************** + * @brief Converts a raw ADC channel mask to a x-y-sorted pixel mask. + * @param msk The raw ADC channel mask to be converted. + * @return The converted x-y-sorted pixel mask. + ******************************************************************************/ +static inline uint32_t ChannelToPixelMask(uint32_t msk) +{ + uint32_t res = 0; + + for (uint_fast8_t n = 0; n < 32; n += 2) { + res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; + } + + return res; +} + +/*! @} */ +#endif /* ARGUS_MAP_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h new file mode 100644 index 000000000000..0e074c6a8b03 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h @@ -0,0 +1,136 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic measurement parameters and data structures. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_MEAS_H +#define ARGUS_MEAS_H + +/*!*************************************************************************** + * @defgroup argusmeas Measurement/Device Control + * @ingroup argusapi + * + * @brief Measurement/Device control module + * + * @details This module contains measurement and device control specific + * definitions and methods. + * + * @addtogroup argusmeas + * @{ + *****************************************************************************/ + +#include "argus_dca.h" +#include "argus_def.h" + +/*! Number of raw data values. */ +#define ARGUS_RAW_DATA_VALUES 132U // 33 channels * 4 phases + +/*! Size of the raw data in bytes. */ +#define ARGUS_RAW_DATA_SIZE (3U * ARGUS_RAW_DATA_VALUES) // 3 bytes * 33 channels * 4 phases + +/*! The number channels for auxiliary measurements readout. */ +#define ARGUS_AUX_CHANNEL_COUNT (5U) + +/*! Size of the auxiliary data in bytes. */ +#define ARGUS_AUX_DATA_SIZE (3U * ARGUS_AUX_CHANNEL_COUNT) // 3 bytes * x channels * 1 phase + +/*!*************************************************************************** + * @brief The device measurement configuration structure. + * @details The portion of the configuration data that belongs to the + * measurement cycle. I.e. the data that defines a measurement frame. + *****************************************************************************/ +typedef struct { + /*! Frame integration time in microseconds. + * The integration time determines the measured time between + * the start signal and the IRQ. Note that this value will be + * slightly larger than the actual integration time since the + * watch is started before the SPI transfer and stopped in the + * IRQ service routine which also might be delayed due to higher + * priority tasks. */ + uint32_t IntegrationTime; + + /*! Pixel enabled mask for the 32 pixels sorted + * by x-y-indices. + * See [pixel mapping](@ref argusmap) for more + * details on the pixel mask. */ + uint32_t PxEnMask; + + /*! ADS channel enabled mask for the remaining + * channels 31 .. 63 (miscellaneous values). + * See [pixel mapping](@ref argusmap) for more + * details on the ADC channel mask. */ + uint32_t ChEnMask; + + /*! The current state of the measurement frame: + * - Measurement Mode, + * - A/B Frame, + * - PLL_Locked Bit, + * - BGL Warning/Error, + * - DCA State, + * - ... */ + argus_state_t State; + + /*! Pattern count per sample in uq10.6 format. + * Determines the analog integration depth. */ + uq10_6_t AnalogIntegrationDepth; + + /*! Sample count per phase/frame. + * Determines the digital integration depth. */ + uint16_t DigitalIntegrationDepth; + + /*! Laser Modulation Current per sample in mA. + * Determines the optical output power. */ + uq12_4_t OutputPower; + + /*! The amplitude that is evaluated and used in the DCA module. */ + uq12_4_t DCAAmplitude; + + /*! Laser Bias Current Settings in LSB. */ + uint8_t BiasCurrent; + + /*! Charge pump voltage per sample in LSB. + * Determines the pixel gain. */ + uint8_t PixelGain; + + /*! PLL Frequency Offset, caused by temperature + * compensation, in PLL_INT_PRD LSBs. */ + int8_t PllOffset; + + /*! The current PLL_CTRL_CUR value. */ + uint8_t PllCtrlCur; + +} argus_meas_frame_t; + +/*! @} */ +#endif /* ARGUS_MEAS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h new file mode 100644 index 000000000000..258fb3826092 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h @@ -0,0 +1,170 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines macros to work with pixel and ADC channel masks. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_MSK_H +#define ARGUS_MSK_H + +/*!*************************************************************************** + * @defgroup argusmap ADC Channel Mapping + * @ingroup argusres + * + * @brief Pixel ADC Channel (n) to x-y-Index Mapping + * + * @details The ADC Channels of each pixel or auxiliary channel on the device + * is numbered in a way that is convenient on the chip. The macros + * in this module are defined in order to obtain the x-y-indices of + * each channel and vice versa. + * + * @addtogroup argusmap + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" +#include "utility/int_math.h" + +/*!***************************************************************************** + * @brief Macro to determine the channel number of an specified Pixel. + * @param x The x index of the pixel. + * @param y The y index of the pixel. + * @return The channel number n of the pixel. + ******************************************************************************/ +#define PIXEL_XY2N(x, y) ((((x) ^ 7) << 1) | ((y) & 2) << 3 | ((y) & 1)) + +/*!***************************************************************************** + * @brief Macro to determine the x index of an specified Pixel channel. + * @param n The channel number of the pixel. + * @return The x index number of the pixel. + ******************************************************************************/ +#define PIXEL_N2X(n) ((((n) >> 1U) & 7) ^ 7) + +/*!***************************************************************************** + * @brief Macro to determine the y index of an specified Pixel channel. + * @param n The channel number of the pixel. + * @return The y index number of the pixel. + ******************************************************************************/ +#define PIXEL_N2Y(n) (((n) & 1U) | (((n) >> 3) & 2U)) + +/*!***************************************************************************** + * @brief Macro to determine if a ADC Pixel channel was enabled from a pixel mask. + * @param msk The 32-bit pixel mask + * @param ch The channel number of the pixel. + * @return True if the pixel channel n was enabled, false elsewise. + ******************************************************************************/ +#define PIXELN_ISENABLED(msk, ch) (((msk) >> (ch)) & 0x01U) + +/*!***************************************************************************** + * @brief Macro enables an ADC Pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param ch The channel number of the pixel. + ******************************************************************************/ +#define PIXELN_ENABLE(msk, ch) ((msk) |= (0x01U << (ch))) + +/*!***************************************************************************** + * @brief Macro disables an ADC Pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param ch The channel number of the pixel. + ******************************************************************************/ +#define PIXELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << (ch)))) + +/*!***************************************************************************** + * @brief Macro to determine if an ADC Pixel channel was enabled from a pixel mask. + * @param msk 32-bit pixel mask + * @param x x index of the pixel. + * @param y y index of the pixel. + * @return True if the pixel (x,y) was enabled, false elsewise. + ******************************************************************************/ +#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y))) + +/*!***************************************************************************** + * @brief Macro enables an ADC Pixel channel in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x index of the pixel. + * @param y y index of the pixel. + ******************************************************************************/ +#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y))) + +/*!***************************************************************************** + * @brief Macro disables an ADC Pixel channel in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x index of the pixel. + * @param y y index of the pixel. + ******************************************************************************/ +#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y))) + +/*!***************************************************************************** + * @brief Macro to determine if a ADC channel was enabled from a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel n was enabled, false elsewise. + ******************************************************************************/ +#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U) + +/*!***************************************************************************** + * @brief Macro to determine if a ADC channel was enabled from a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel n was enabled, false elsewise. + ******************************************************************************/ +#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U))) + +/*!***************************************************************************** + * @brief Macro to determine if a ADC channel was enabled from a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel n was enabled, false elsewise. + ******************************************************************************/ +#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U)))) + + +/*!***************************************************************************** + * @brief Macro to determine the number of enabled pixel channels via a popcount + * algorithm. + * @param pxmsk 32-bit pixel mask + * @return The count of enabled pixel channels. + ******************************************************************************/ +#define PIXEL_COUNT(pxmsk) popcount(pxmsk) + +/*!***************************************************************************** + * @brief Macro to determine the number of enabled channels via a popcount + * algorithm. + * @param pxmsk 32-bit pixel mask + * @param chmsk 32-bit channel mask + * @return The count of enabled ADC channels. + ******************************************************************************/ +#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk)) + +/*! @} */ +#endif /* ARGUS_MSK_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h new file mode 100644 index 000000000000..07b4853bda76 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h @@ -0,0 +1,221 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the pixel binning algorithm (PBA) setup parameters and + * data structure. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_PBA_H +#define ARGUS_PBA_H + +/*!*************************************************************************** + * @defgroup arguspba Pixel Binning Algorithm + * @ingroup argusapi + * + * @brief Pixel Binning Algorithm (PBA) parameter definitions and API functions. + * + * @details Defines the generic pixel binning algorithm (PBA) setup parameters + * and data structure. + * + * The PBA module contains filter algorithms that determine the + * pixels with the best signal quality and extract an 1d distance + * information from the filtered pixels. + * + * The pixel filter algorithm is a three-stage filter with a + * fallback value: + * + * -# A fixed pre-filter mask is applied to statically disable + * specified pixels. + * -# A relative and absolute amplitude filter is applied in the + * second stage. The relative filter is determined by a ratio + * of the maximum amplitude off all available (i.e. not filtered + * in stage 1) pixels. Pixels that have an amplitude below the + * relative threshold are dismissed. The same holds true for + * the absolute amplitude threshold. All pixel with smaller + * amplitude are dismissed.\n + * The relative threshold is useful to setup a distance + * measurement scenario. All well illuminated pixels are + * selected and considered for the final 1d distance. The + * absolute threshold is used to dismiss pixels that are below + * the noise level. The latter would be considered for the 1d + * result if the maximum amplitude is already very low. + * -# A distance filter is used to distinguish pixels that target + * the actual object from pixels that see the brighter background, + * e.g. white walls. Thus, the pixel with the minimum distance + * is referenced and all pixel that have a distance between + * the minimum and the given minimum distance scope are selected + * for the 1d distance result. The minimum distance scope is + * determined by an relative (to the current minimum distance) + * and an absolute value. The larger scope value is the + * relevant one, i.e. the relative distance scope can be used + * to heed the increasing noise at larger distances. + * -# If all of the above filters fail to determine a single valid + * pixel, the golden pixel is used as a fallback value. The + * golden pixel is the pixel that sits right at the focus point + * of the optics at large distances. + * . + * + * After filtering is done, there may be more than a single pixel + * left to determine the 1d signal. Therefore several averaging + * methods are implemented to obtain the best 1d result from many + * pixels. See #argus_pba_averaging_mode_t for details. + * + * + * @addtogroup arguspba + * @{ + *****************************************************************************/ + +#include "argus_def.h" + +/*!*************************************************************************** + * @brief Enable flags for the pixel binning algorithm. + * + * @details Determines the pixel binning algorithm feature enable status. + * - [0]: #PBA_ENABLE: Enables the pixel binning feature. + * - [1]: reserved + * - [2]: reserved + * - [3]: reserved + * - [4]: reserved + * - [5]: #PBA_ENABLE_GOLDPX: Enables the golden pixel feature. + * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope + * feature. + * - [7]: reserved + * . + *****************************************************************************/ +typedef enum { + /*! Enables the pixel binning feature. */ + PBA_ENABLE = 1U << 0U, + + /*! Enables the golden pixel. */ + PBA_ENABLE_GOLDPX = 1U << 5U, + + /*! Enables the minimum distance scope filter. */ + PBA_ENABLE_MIN_DIST_SCOPE = 1U << 6U, + +} argus_pba_flags_t; + +/*!*************************************************************************** + * @brief The averaging modes for the pixel binning algorithm. + *****************************************************************************/ +typedef enum { + /*! Evaluate the 1D range from all available pixels using + * a simple average. */ + PBA_SIMPLE_AVG = 1U, + + /*! Evaluate the 1D range from all available pixels using + * a linear amplitude weighted averaging method. + * Formula: x_mean = sum(x_i * A_i) / sum(A_i) */ + PBA_LINEAR_AMPLITUDE_WEIGHTED_AVG = 2U, + +} argus_pba_averaging_mode_t; + +/*!*************************************************************************** + * @brief The pixel binning algorithm settings data structure. + * @details Describes the pixel binning algorithm settings. + *****************************************************************************/ +typedef struct { + /*! Enables the pixel binning features. + * Each bit may enable a different feature. See #argus_pba_flags_t + * for details about the enabled flags. */ + argus_pba_flags_t Enabled; + + /*! Determines the PBA averaging mode which is used to obtain the + * final range value from the algorithm, for example, the average + * of all pixels. See #argus_pba_averaging_mode_t for more details + * about the individual evaluation modes. */ + argus_pba_averaging_mode_t AveragingMode; + + /*! The Relative amplitude threshold value (in %) of the max. amplitude. + * Pixels with amplitude below this threshold value are dismissed. + * + * All available values from the 8-bit representation are valid. + * The actual percentage value is determined by 100%/256*x. + * + * Use 0 to disable the relative amplitude threshold. */ + uq0_8_t RelAmplThreshold; + + /*! The relative minimum distance scope value in %. + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. + * + * All available values from the 8-bit representation are valid. + * The actual percentage value is determined by 100%/256*x. + * + * Special values: + * - 0: Use 0 for absolute value only or to choose the pixel with the + * minimum distance only (of also the absolute value is 0)! */ + uq0_8_t RelMinDistanceScope; + + /*! The Absolute amplitude threshold value in LSB. + * Pixels with amplitude below this threshold value are dismissed. + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/16. + * + * Use 0 to disable the absolute amplitude threshold. */ + uq12_4_t AbsAmplThreshold; + + /*! The absolute minimum distance scope value in m. + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. + * + * All available values from the 16-bit representation are valid. + * The actual LSB value is determined by x/2^15. + * + * Special values: + * - 0: Use 0 for relative value only or to choose the pixel with the + * minimum distance only (of also the relative value is 0)! */ + uq1_15_t AbsMinDistanceScope; + + /*! The pre-filter pixel mask determines the pixel channels that are + * statically excluded from the pixel binning (i.e. 1D distance) result. + * + * The pixel enabled mask is an 32-bit mask that determines the + * device internal channel number. It is recommended to use the + * - #PIXELXY_ISENABLED(msk, x, y) + * - #PIXELXY_ENABLE(msk, x, y) + * - #PIXELXY_DISABLE(msk, x, y) + * . + * macros to work with the pixel enable masks. */ + uint32_t PrefilterMask; + +} argus_cfg_pba_t; + +/*! @} */ +#endif /* ARGUS_PBA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h new file mode 100644 index 000000000000..faa031aeb733 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h @@ -0,0 +1,154 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the device pixel measurement results data structure. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_PX_H +#define ARGUS_PX_H + +/*!*************************************************************************** + * @addtogroup argusres + * @{ + *****************************************************************************/ + +/*! Maximum amplitude value in UQ12.4 format. */ +#define ARGUS_AMPLITUDE_MAX (0xFFF0U) + +/*! Maximum range value in Q9.22 format. + * Also used as a special value to determine no object detected or infinity range. */ +#define ARGUS_RANGE_MAX (Q9_22_MAX) + +/*!*************************************************************************** + * @brief Status flags for the evaluated pixel structure. + * + * @details Determines the pixel status. 0 means OK (#PIXEL_OK). + * - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device. + * - [1]: #PIXEL_SAT: The pixel was saturated. + * - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result. + * - [3]: #PIXEL_AMPL_MIN: The pixel amplitude has evaluated to 0. + * - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask. + * - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal. + * - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace. + * - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors. + * . + *****************************************************************************/ +typedef enum { + /*! 0x00: Pixel status OK. */ + PIXEL_OK = 0, + + /*! 0x01: Pixel is disabled (in hardware) and no data has been read from the device. */ + PIXEL_OFF = 1U << 0U, + + /*! 0x02: Pixel is saturated (i.e. at least one saturation bit for any + * sample is set or the sample is in the invalidity area). */ + PIXEL_SAT = 1U << 1U, + + /*! 0x04: Pixel is excluded from the pixel binning (1d) result. */ + PIXEL_BIN_EXCL = 1U << 2U, + + /*! 0x08: Pixel amplitude minimum underrun + * (i.e. the amplitude calculation yields 0). */ + PIXEL_AMPL_MIN = 1U << 3U, + + /*! 0x10: Pixel is pre-filtered by the static pixel binning pre-filter mask, + * i.e. the pixel is disabled by software. */ + PIXEL_PREFILTERED = 1U << 4U, + + /*! 0x20: Pixel amplitude is below its threshold value. The received signal + * strength is too low to evaluate a valid signal. The range value is + * set to the maximum possible value (approx. 512 m). */ + PIXEL_NO_SIGNAL = 1U << 5U, + + /*! 0x40: Pixel is not in sync with respect to the dual frequency algorithm. + * I.e. the pixel may have a correct value but is estimated into the + * wrong unambiguous window. */ + PIXEL_OUT_OF_SYNC = 1U << 6U, + + /*! 0x80: Pixel is stalled due to one of the following reasons: + * - #PIXEL_SAT + * - #PIXEL_AMPL_MIN + * - #PIXEL_OUT_OF_SYNC + * - Global Measurement Error + * . + * A stalled pixel does not update its measurement data and keeps the + * previous values. If the issue is resolved, the stall disappears and + * the pixel is updating again. */ + PIXEL_STALLED = 1U << 7U + +} argus_px_status_t; + +/*!*************************************************************************** + * @brief The evaluated measurement results per pixel. + * @details This structure contains the evaluated data for a single pixel.\n + * If the amplitude is 0, the pixel is turned off or has invalid data. + *****************************************************************************/ +typedef struct { + /*! Range Values from the device in meter. It is the actual distance before + * software adjustments/calibrations. */ + q9_22_t Range; + + /*! Phase Values from the device in units of PI, i.e. 0 ... 2. */ + uq1_15_t Phase; + + /*! Amplitudes of measured signals in LSB. + * Special values: 0 == Pixel Off, 0xFFFF == Overflow/Error */ + uq12_4_t Amplitude; + + /*! Pixel status; determines if the pixel is disabled, saturated, .. + * See the \link #argus_px_status_t pixel status flags\endlink for more + * information. */ + argus_px_status_t Status; + + /*! The unambiguous window determined by the dual frequency feature. */ + int8_t RangeWindow; + + /*! The raw amplitudes of measured signals in LSB. */ + uq12_4_t AmplitudeRaw; + +} argus_pixel_t; + +/*!*************************************************************************** + * @brief Representation of a correlation vector containing sine/cosine components. + *****************************************************************************/ +typedef struct { + /*! The sine component. */ + q15_16_t S; + + /*! The cosine component. */ + q15_16_t C; + +} argus_vector_t; + +/*! @} */ +#endif /* ARGUS_PX_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h new file mode 100644 index 000000000000..f59d8176347b --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h @@ -0,0 +1,220 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the generic measurement results data structure. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_RES_H +#define ARGUS_RES_H + +/*!*************************************************************************** + * @defgroup argusres Measurement Data + * @ingroup argusapi + * + * @brief Measurement results data structures. + * + * @details The interface defines all data structures that correspond to + * the AFBR-S50 measurement results, e.g. + * - 1D distance and amplitude values, + * - 3D distance and amplitude values (i.e. per pixel), + * - Auxiliary channel measurement results (VDD, IAPD, temperature, ...) + * - Device and result status + * - ... + * . + * + * @addtogroup argusres + * @{ + *****************************************************************************/ + +#include "argus_def.h" +#include "argus_px.h" +#include "argus_meas.h" + +/*!*************************************************************************** + * @brief The 1d measurement results data structure. + * @details The 1d measurement results obtained by the Pixel Binning Algorithm. + *****************************************************************************/ +typedef struct { + /*! Raw 1D range value in meter (Q9.22 format). The distance obtained by + * the Pixel Binning Algorithm from the current measurement frame. */ + q9_22_t Range; + + /*! The 1D amplitude in LSB (Q12.4 format). The (maximum) amplitude obtained + * by the Pixel Binning Algorithm from the current measurement frame.\n + * Special value: 0 == No/Invalid Result. */ + uq12_4_t Amplitude; + + /*! The current signal quality metric of the 1D range value in percentage:\n + * - 0: n/a, + * - 1: bad signal, + * - 100: good signal. */ + uint8_t SignalQuality; + +} argus_results_bin_t; + +/*!*************************************************************************** + * @brief The auxiliary measurement results data structure. + * @details The auxiliary measurement results obtained by the auxiliary task.\n + * Special values, i.e. 0xFFFFU, indicate no readout value available. + *****************************************************************************/ +typedef struct { + /*! VDD ADC channel readout value.\n + * Special Value if no value has been measured:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t VDD; + + /*! Temperature sensor ADC channel readout value.\n + * Special Value if no value has been measured:\n + * Invalid/NotAvailable = 0x7FFFU (Q11_4_MAX) */ + q11_4_t TEMP; + + /*! Substrate Voltage ADC Channel readout value.\n + * Special Value if no value has been measured:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t VSUB; + + /*! VDD VCSEL ADC channel readout value.\n + * Special Value if no value has been measured:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t VDDL; + + /*! APD current ADC Channel readout value.\n + * Special Value if no value has been measured:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t IAPD; + + /*! Background Light Value in arbitrary. units, + * estimated by the substrate voltage control task.\n + * Special Value if no value is available:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t BGL; + + /*! Shot Noise Amplitude in LSB units, + * estimated by the shot noise monitor task from + * the average amplitude of the passive pixels.\n + * Special Value if no value is available:\n + * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ + uq12_4_t SNA; + +} argus_results_aux_t; + +/*!*************************************************************************** + * @brief The measurement results data structure. + * @details This structure contains all information obtained by a single + * distance measurement on the device: + * - The measurement status can be read from the #Status. + * - A timing information is given via the #TimeStamp. + * - Information about the frame state is in the #Frame structure. + * - The 1D distance results are gathered under #Bin. + * - The 3D distance results for each pixel is at #Pixels or #Pixel. + * - Auxiliary values such as temperature can be found at #Auxiliary. + * - Raw data from the device is stored in the #Data array. + * . + * + * The pixel x-y orientation is sketched in the following graph. Note that + * the laser source would be on the right side beyond the reference pixel. + * See also \link argusmap ADC Channel Mapping\endlink + * @code + * // Pixel Field: Pixel[x][y] + * // + * // 0 -----------> x + * // | O O O O O O O O + * // | O O O O O O O O + * // | O O O O O O O O O (ref. Px) + * // y O O O O O O O O + * @endcode + *****************************************************************************/ +typedef struct { + /*! The \link #status_t status\endlink of the current measurement frame. + * - 0 (i.e. #STATUS_OK) for a good measurement signal. + * - > 0 for warnings and weak measurement signal. + * - < 0 for errors and invalid measurement signal. */ + status_t Status; + + /*! Time in milliseconds (measured since the last MCU startup/reset) + * when the measurement was triggered. */ + ltc_t TimeStamp; + + /*! The configuration for the current measurement frame. */ + argus_meas_frame_t Frame; + + /*! Raw x-y-sorted ADC results from the device.\n + * Data is arranged as 32-bit values in following order: + * index > phase; where index is pixel number n and auxiliary ADC channel.\n + * Note that disabled pixels are skipped.\n + * e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */ + uint32_t Data[ARGUS_RAW_DATA_VALUES]; + + union { + /*! Pixel data indexed by channel number n.\n + * Contains calibrated range, amplitude and pixel status among others. + * + * Index n: + * - 0..31: active pixels + * - 32: reference pixel + * + * See also \link argusmap ADC Channel Mapping\endlink */ + argus_pixel_t Pixels[ARGUS_PIXELS + 1U]; + + struct { + /*! Pixel data indexed by x-y-indices.\n + * The pixels are ordered in a two dimensional array that represent + * the x and y indices of the pixel.\n + * See also \link argusmap ADC Channel Mapping\endlink + * + * Contains calibrated range, amplitude and pixel status among others. */ + argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + + /*! Pixel data of the reference pixel.\n + * The reference pixel is an additional pixel that is located at the TX + * side in order to monitor the health state of the laser output source. + * It is mainly used to verify normal operation of the laser source and + * preventing the system from emitting continuous laser light that exceeds + * the laser safety limits. + * + * Contains calibrated range, amplitude and pixel status among others. */ + argus_pixel_t PixelRef; + }; + }; + + /*! The 1D measurement data, obtained by the the Pixel Binning Algorithm. */ + argus_results_bin_t Bin; + + /*! The auxiliary ADC channel data, e.g. sensor temperature. */ + argus_results_aux_t Auxiliary; + +} argus_results_t; + + +/*! @} */ +#endif /* ARGUS_RES_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h new file mode 100644 index 000000000000..277630814396 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h @@ -0,0 +1,82 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Defines the Shot Noise Monitor (SNM) setup parameters. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_SNM_H +#define ARGUS_SNM_H + +/*!*************************************************************************** + * @defgroup argussnm Shot Noise Monitor + * @ingroup argusapi + * + * @brief Shot Noise Monitor (SNM) parameter definitions and API functions. + * + * @details The SNM is an algorithm to monitor and react on shot noise + * induced by harsh environment conditions like high ambient + * light. + * + * The AFBR-S50 API provides three modes: + * - Dynamic: Automatic mode, automatically adopts to current + * ambient conditions. + * - Static (Outdoor): Static mode, optimized for outdoor applications. + * - Static (Indoor): Static mode, optimized for indoor applications. + * . + * + * @addtogroup argussnm + * @{ + *****************************************************************************/ + +/*! The Shot Noise Monitor modes enumeration. */ +typedef enum { + /*! Static Shot Noise Monitoring Mode, optimized for indoor applications. + * Assumes the best case scenario, i.e. no bad influence from ambient conditions. + * Thus it uses a fixed setting that will result in the best performance. + * Equivalent to Shot Noise Monitoring disabled. */ + SNM_MODE_STATIC_INDOOR = 0U, + + /*! Static Shot Noise Monitoring Mode, optimized for outdoor applications. + * Assumes the worst case scenario, i.e. it uses a fixed setting that will + * work under all ambient conditions. */ + SNM_MODE_STATIC_OUTDOOR = 1U, + + /*! Dynamic Shot Noise Monitoring Mode. + * Adopts the system performance dynamically to the current ambient conditions. */ + SNM_MODE_DYNAMIC = 2U, + +} argus_snm_mode_t; + + +/*! @} */ +#endif /* ARGUS_SNM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h new file mode 100644 index 000000000000..244ad1beec7e --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h @@ -0,0 +1,268 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides status codes for the AFBR-S50 API. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_STATUS_H +#define ARGUS_STATUS_H + +#include + +/*!*************************************************************************** + * @defgroup status Status Codes + * @brief Status and Error Code Definitions + * @details Defines status and error codes for function return values. + * Basic status number structure: + * - 0 is OK or no error. + * - negative values determine errors. + * - positive values determine warnings or status information. + * . + * @addtogroup status + * @{ + *****************************************************************************/ + +/*!*************************************************************************** + * @brief Type used for all status and error return values. + * @details Basic status number structure: + * - 0 is OK or no error. + * - negative values determine errors. + * - positive values determine warnings or status information. + * . + *****************************************************************************/ +typedef int32_t status_t; + +/*! AFBR-S50 API status and error return codes. */ +enum Status { + /********************************************************************************************** + ********** Generic Status ******************************************************************** + *********************************************************************************************/ + + /*! 0: Status for success/no error. */ + STATUS_OK = 0, + + /*! 0: Status for device/module/hardware idle. Implies #STATUS_OK. */ + STATUS_IDLE = 0, + + /*! 1: Status to be ignored. */ + STATUS_IGNORE = 1, + + /*! 2: Status for device/module/hardware busy. */ + STATUS_BUSY = 2, + + /*! 3: Status for device/module/hardware is currently initializing. */ + STATUS_INITIALIZING = 3, + + /*! -1: Error for generic fail/error. */ + ERROR_FAIL = -1, + + /*! -2: Error for process aborted by user/external. */ + ERROR_ABORTED = -2, + + /*! -3: Error for invalid read only operations. */ + ERROR_READ_ONLY = -3, + + /*! -4: Error for out of range parameters. */ + ERROR_OUT_OF_RANGE = -4, + + /*! -5: Error for invalid argument passed to an function. */ + ERROR_INVALID_ARGUMENT = -5, + + /*! -6: Error for timeout occurred. */ + ERROR_TIMEOUT = -6, + + /*! -7: Error for not initialized modules. */ + ERROR_NOT_INITIALIZED = -7, + + /*! -8: Error for not supported. */ + ERROR_NOT_SUPPORTED = -8, + + /*! -9: Error for yet not implemented functions. */ + ERROR_NOT_IMPLEMENTED = -9, + + + /********************************************************************************************** + ********** S2PI Layer Status ***************************************************************** + *********************************************************************************************/ + + /*! 51: SPI is disabled and pins are used in GPIO mode. */ + STATUS_S2PI_GPIO_MODE = 51, + + /*! -51: Error occurred on the Rx line. */ + ERROR_S2PI_RX_ERROR = -51, + + /*! -52: Error occurred on the Tx line. */ + ERROR_S2PI_TX_ERROR = -52, + + /*! -53: Called a function at a wrong driver state. */ + ERROR_S2PI_INVALID_STATE = -53, + + /*! -54: The specified baud rate is not valid. */ + ERROR_S2PI_INVALID_BAUDRATE = -54, + + /*! -55: The specified slave identifier is not valid. */ + ERROR_S2PI_INVALID_SLAVE = -55, + + + /********************************************************************************************** + ********** NVM / Flash Layer Status ********************************************************* + *********************************************************************************************/ + + /*! -98: Flash Error: The version of the settings in the flash memory is not compatible. */ + ERROR_NVM_INVALID_FILE_VERSION = -98, + + /*! -99: Flash Error: The memory is out of range. */ + ERROR_NVM_OUT_OF_RANGE = -99, + + + /********************************************************************************************** + ********** AFBR-S50 Specific Status ********************************************************** + *********************************************************************************************/ + + /*! 104: AFBR-S50 Status: All (internal) raw data buffers are currently in use. + * The measurement was not executed due to lack of available raw data buffers. + * Please call #Argus_EvaluateData to free the buffers. */ + STATUS_ARGUS_BUFFER_BUSY = 104, + + /*! 105: AFBR-S50 Status: The measurement was not executed/started due to output power + * limitations. */ + STATUS_ARGUS_POWERLIMIT = 105, + + /*! 106: AFBR-S50 Status: The PLL was not locked when the measurement was + * started. The measurement frequency and this the range might be off. */ + STATUS_ARGUS_PLL_NOT_LOCKED = 106, + + /*! 108: AFBR-S50 Status: No object was detected within the field-of-view + * and measurement range of the device. */ + STATUS_ARGUS_NO_OBJECT = 108, + + /*! 109: AFBR-S50 Status: The readout algorithm for the EEPROM has detected a bit + * error which has been corrected. However, if more than a single bit error + * has occurred, the corrected value is invalid! This cannot be distinguished + * from the valid case. Thus, if the error starts to occur, the sensor + * should be replaced soon! */ + STATUS_ARGUS_EEPROM_BIT_ERROR = 109, + + /*! 110: AFBR-S50 Status: Inconsistent EEPROM readout data. No calibration + * trimming values are applied. The calibration remains invalid. */ + STATUS_ARGUS_INVALID_EEPROM = 110, + + /*! -101: AFBR-S50 Error: No device connected. Initial SPI tests failed. */ + ERROR_ARGUS_NOT_CONNECTED = -101, + + /*! -102: AFBR-S50 Error: Inconsistent configuration parameters. */ + ERROR_ARGUS_INVALID_CFG = -102, + + + /*! -105: AFBR-S50 Error: Invalid measurement mode configuration parameter. */ + ERROR_ARGUS_INVALID_MODE = -105, + + /*! -107: AFBR-S50 Error: The APD bias voltage is reinitializing due to a dropout. + * The current measurement data set is invalid! */ + ERROR_ARGUS_BIAS_VOLTAGE_REINIT = -107, + + + /*! -109: AFBR-S50 Error: The EEPROM readout has failed. The failure is detected + * by three distinct read attempts, each resulting in invalid data. + * Note: this state differs from that #STATUS_ARGUS_EEPROM_BIT_ERROR + * such that it is usually temporarily and due to harsh ambient conditions. */ + ERROR_ARGUS_EEPROM_FAILURE = -109, + + /*! -110: AFBR-S50 Error: The measurement signals of all active pixels are invalid + * and thus the 1D range is also invalid and stalled. + * This means the range value is not updated and kept at the previous valid value. */ + ERROR_ARGUS_STALLED = -110, + + /*! -111: AFBR-S50 Error: The background light is too bright. */ + ERROR_ARGUS_BGL_EXCEEDANCE = -111, + + /*! -112: AFBR-S50 Error: The crosstalk vector amplitude is too high. */ + ERROR_ARGUS_XTALK_AMPLITUDE_EXCEEDANCE = -112, + + /*! -113: AFBR-S50 Error: Laser malfunction! Laser Safety may not be given! */ + ERROR_ARGUS_LASER_FAILURE = -113, + + /*! -114: AFBR-S50 Error: Register data integrity is lost (e.g. due to unexpected + * power-on-reset cycle or invalid write cycle of SPI. System tries to + * reset the values. */ + ERROR_ARGUS_DATA_INTEGRITY_LOST = -114, + + /*! -115: AFBR-S50 Error: The range offsets calibration failed! */ + ERROR_ARGUS_RANGE_OFFSET_CALIBRATION_FAILED = -115, + + /*! -191: AFBR-S50 Error: The device is currently busy and cannot execute the + * requested command. */ + ERROR_ARGUS_BUSY = -191, + + + /*! -199: AFBR-S50 Error: Unknown module number. */ + ERROR_ARGUS_UNKNOWN_MODULE = -199, + + /*! -198: AFBR-S50 Error: Unknown chip version number. */ + ERROR_ARGUS_UNKNOWN_CHIP = -198, + + /*! -197: AFBR-S50 Error: Unknown laser type number. */ + ERROR_ARGUS_UNKNOWN_LASER = -197, + + + + /*! 193: AFBR-S50 Status (internal): The device is currently busy with updating the + * configuration (i.e. with writing register values). */ + STATUS_ARGUS_BUSY_CFG_UPDATE = 193, + + /*! 194: AFBR-S50 Status (internal): The device is currently busy with updating the + * calibration data (i.e. writing to register values). */ + STATUS_ARGUS_BUSY_CAL_UPDATE = 194, + + /*! 195: AFBR-S50 Status (internal): The device is currently executing a calibration + * sequence. */ + STATUS_ARGUS_BUSY_CAL_SEQ = 195, + + /*! 196: AFBR-S50 Status (internal): The device is currently executing a measurement + * cycle. */ + STATUS_ARGUS_BUSY_MEAS = 196, + + + /*! 100: AFBR-S50 Status (internal): The ASIC is initializing a new measurement, i.e. + * a register value is written that starts an integration cycle on the ASIC. */ + STATUS_ARGUS_STARTING = 100, + + /*! 103: AFBR-S50 Status (internal): The ASIC is performing an integration cycle. */ + STATUS_ARGUS_ACTIVE = 103, + + + +}; + +/*! @} */ +#endif /* ARGUS_STATUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h new file mode 100644 index 000000000000..ea16342c843d --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h @@ -0,0 +1,76 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file contains the current API version number. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_VERSION_H +#define ARGUS_VERSION_H + +/*!*************************************************************************** + * @defgroup version API Version + * @ingroup argusapi + * + * @brief API and library core version number + * + * @details Contains the AFBR-S50 API and Library Core Version Number. + * + * @addtogroup version + * @{ + *****************************************************************************/ + +/*! Major version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_MAJOR 1 + +/*! Minor version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_MINOR 3 + +/*! Bugfix version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_BUGFIX 5 + +/*! Build version nunber of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_BUILD "20210812171515" + +/*****************************************************************************/ + +/*! Construct the version number for drivers. */ +#define MAKE_VERSION(major, minor, bugfix) \ + (((major) << 24) | ((minor) << 16) | (bugfix)) + +/*! Version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION MAKE_VERSION((ARGUS_API_VERSION_MAJOR), \ + (ARGUS_API_VERSION_MINOR), \ + (ARGUS_API_VERSION_BUGFIX)) + +/*! @} */ +#endif /* ARGUS_VERSION_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h new file mode 100644 index 000000000000..561370626736 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h @@ -0,0 +1,114 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic device calibration API. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_XTALK_H +#define ARGUS_XTALK_H + +/*!*************************************************************************** + * @addtogroup arguscal + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" + +/*!*************************************************************************** + * @brief Pixel Crosstalk Compensation Vector. + * @details Contains calibration data (per pixel) that belongs to the + * RX-TX-Crosstalk compensation feature. + *****************************************************************************/ + +/*! Pixel Crosstalk Vector */ +typedef struct { + /*! Crosstalk Vector - Sine component. + * Special Value: Q11_4_MIN == not available */ + q11_4_t dS; + + /*! Crosstalk Vector - Cosine component. + * Special Value: Q11_4_MIN == not available */ + q11_4_t dC; + +} xtalk_t; + +/*!*************************************************************************** + * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains calibration data that belongs to the pixel-to-pixel + * crosstalk compensation feature. + *****************************************************************************/ +typedef struct { + /*! Pixel-To-Pixel Compensation on/off. */ + bool Enabled; + + /*! The relative threshold determines when the compensation is active for + * each individual pixel. The value determines the ratio of the individual + * pixel signal is with respect to the overall average signal. If the + * ratio is smaller than the value, the compensation is active. Absolute + * and relative conditions are connected with AND logic. */ + uq0_8_t RelativeThreshold; + + /*! The absolute threshold determines the minimum total crosstalk + * amplitude (i.e. the average amplitude of all pixels weighted by + * the Kc factor) that is required for the compensation to become + * active. Set to 0 to always enable. Absolute and relative + * conditions are connected with AND logic. */ + uq12_4_t AbsoluteTreshold; + + /*! The sine component of the Kc factor that determines the amount of the total + * signal of all pixels that influences the individual signal of each pixel. + * Higher values determine more influence on the individual pixel signal. */ + q3_12_t KcFactorS; + + /*! The cosine component of the Kc factor that determines the amount of the total + * signal of all pixels that influences the individual signal of each pixel. + * Higher values determine more influence on the individual pixel signal. */ + q3_12_t KcFactorC; + + /*! The sine component of the reference pixel Kc factor that determines the + * amount of the total signal on all pixels that influences the individual + * signal of the reference pixel. + * Higher values determine more influence on the reference pixel signal. */ + q3_12_t KcFactorSRefPx; + + /*! The cosine component of the reference pixel Kc factor that determines the + * amount of the total signal on all pixels that influences the individual + * signal of the reference pixel. + * Higher values determine more influence on the reference pixel signal. */ + q3_12_t KcFactorCRefPx; + +} argus_cal_p2pxtalk_t; + + +/*! @} */ +#endif /* ARGUS_XTALK_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h new file mode 100644 index 000000000000..dcea881d02cc --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h @@ -0,0 +1,50 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file the main header of the AFBR-S50 API. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_H +#define ARGUS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "api/argus_api.h" + +#ifdef __cplusplus +} +#endif + +#endif /* ARGUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h new file mode 100644 index 000000000000..3eb7d2cfd4a8 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h @@ -0,0 +1,131 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for enabling/disabling interrupts. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_IRQ_H +#define ARGUS_IRQ_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup argus_irq IRQ: Global Interrupt Control Layer + * @ingroup argus_platform + * + * @brief Global Interrupt Control Layer + * + * @details This module provides functionality to globally enable/disable + * interrupts in a nested way. + * + * Here is a simple example implementation using the CMSIS functions + * "__enable_irq()" and "__disable_irq()". An integer counter is + * used to achieve nested interrupt disabling: + * + * @code + * + * // Global lock level counter value. + * static volatile int g_irq_lock_ct; + * + * // Global unlock all interrupts using CMSIS function "__enable_irq()". + * void IRQ_UNLOCK(void) + * { + * assert(g_irq_lock_ct > 0); + * if (--g_irq_lock_ct <= 0) + * { + * g_irq_lock_ct = 0; + * __enable_irq(); + * } + * } + * + * // Global lock all interrupts using CMSIS function "__disable_irq()". + * void IRQ_LOCK(void) + * { + * __disable_irq(); + * g_irq_lock_ct++; + * } + * + * @endcode + * + * @note The IRQ locking mechanism is used to create atomic sections + * (within the scope of the AFBR-S50 API) that are very few processor + * instruction only. It does NOT lock interrupts for considerable + * amounts of time. + * + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. + * + * @note The interrupts utilized by the AFBR-S50 API can be interrupted + * by other, higher prioritized interrupts, e.g. some system + * critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK + * mechanism can be implemented such that only the interrupts + * required for the AFBR-S50 API are locked. The above example is + * dedicated to a ARM Corex-M0 architecture, where interrupts + * can only disabled at a global scope. Other architectures like + * ARM Cortex-M4 allow selective disabling of interrupts. + * + * @addtogroup argus_irq + * @{ + *****************************************************************************/ + +/*!*************************************************************************** + * @brief Enable IRQ Interrupts + * + * @details Enables IRQ interrupts and enters an atomic or critical section. + * + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. + *****************************************************************************/ +void IRQ_UNLOCK(void); + +/*!*************************************************************************** + * @brief Disable IRQ Interrupts + * + * @details Disables IRQ interrupts and leaves the atomic or critical section. + * + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. + *****************************************************************************/ +void IRQ_LOCK(void); + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif // ARGUS_IRQ_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h new file mode 100644 index 000000000000..69939b775984 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h @@ -0,0 +1,134 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional non-volatile memory. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_NVM_H +#define ARGUS_NVM_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup argus_nvm NVM: Non-Volatile Memory Layer + * @ingroup argus_platform + * + * @brief Non-Volatile Memory Layer + * + * @details This module provides functionality to access the non-volatile + * memory (e.g. flash) on the underlying platform. + * + * This module is optional and only required if calibration data + * needs to be stored within the API. + * + * @note The implementation of this module is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @addtogroup argus_nvm + * @{ + *****************************************************************************/ + +#include "argus.h" + +/*!*************************************************************************** + * @brief Initializes the non-volatile memory unit and reserves a chunk of memory. + * + * @details The function is called upon API initialization sequence. If available, + * the non-volatile memory module reserves a chunk of memory with the + * provides number of bytes (size) and returns with #STATUS_OK. + * + * If not implemented, the function should return #ERROR_NOT_IMPLEMENTED + * in oder to inform the API to not use the NVM module. + * + * After initialization, the API calls the #NVM_Write and #NVM_Read + * methods to write within the reserved chunk of memory. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param size The required size of NVM to store all parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t NVM_Init(uint32_t size); + +/*!*************************************************************************** + * @brief Write a block of data to the non-volatile memory. + * + * @details The function is called whenever the API wants to write data into + * the previously reserved (#NVM_Init) memory block. The data shall + * be written at a given offset and with a given size. + * + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param offset The index offset where the first byte needs to be written. + * @param size The number of bytes to be written. + * @param buf The pointer to the data buffer with the data to be written. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t NVM_Write(uint32_t offset, uint32_t size, uint8_t const *buf); + +/*!*************************************************************************** + * @brief Reads a block of data from the non-volatile memory. + * + * @details The function is called whenever the API wants to read data from + * the previously reserved (#NVM_Init) memory block. The data shall + * be read at a given offset and with a given size. + * + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param offset The index offset where the first byte needs to be read. + * @param size The number of bytes to be read. + * @param buf The pointer to the data buffer to copy the data to. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t NVM_Read(uint32_t offset, uint32_t size, uint8_t *buf); +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif // ARGUS_NVM_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h new file mode 100644 index 000000000000..d53b2df35a90 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h @@ -0,0 +1,83 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional debug module. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_PRINT_H +#define ARGUS_PRINT_H + +/*!*************************************************************************** + * @defgroup argus_log Debug: Logging Interface + * @ingroup argus_platform + * + * @brief Logging interface for the AFBR-S50 API. + * + * @details This interface provides logging utility functions. + * Defines a printf-like function that is used to print error and + * log messages. + * + * @addtogroup argus_log + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" + +/*!*************************************************************************** + * @brief A printf-like function to print formated data to an debugging interface. + * + * @details Writes the C string pointed by fmt_t to an output. If format + * includes format specifiers (subsequences beginning with %), the + * additional arguments following fmt_t are formatted and inserted in + * the resulting string replacing their respective specifiers. + * + * To enable the print functionality, an implementation of the function + * must be provided that maps the output to an interface like UART or + * a debugging console, e.g. by forwarding to standard printf() method. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that does nothing. This will improve + * the performance but no error messages are logged. + * + * @note The naming is different from the standard printf() on purpose to + * prevent builtin compiler optimizations. + * + * @param fmt_s The usual print() format string. + * @param ... The usual print() parameters. * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t print(const char *fmt_s, ...); + +/*! @} */ +#endif /* ARGUS_PRINT_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h new file mode 100644 index 000000000000..4b8f5331b50c --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h @@ -0,0 +1,355 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required S2PI module. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_S2PI_H +#define ARGUS_S2PI_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup argus_s2pi S2PI: Serial Peripheral Interface + * @ingroup argus_platform + * + * @brief S2PI: SPI incl. GPIO Hardware Layer Module + * + * @details The S2PI module consists of a standard SPI interface plus a + * single GPIO interrupt line. Furthermore, the SPI pins are + * accessible via GPIO control to allow a software emulation of + * additional protocols using the same pins. + * + * **SPI interface:** + * + * The SPI interface is based around a single functionality: + * + * #S2PI_TransferFrame. This function transfers a specified number + * of bytes via the interfaces MOSI line and simultaneously reads + * the incoming data on the MOSI line. The read can also be skipped. + * The transfer happen asynchronously, e.g. via a DMA request. After + * finishing the transfer, the provided callback is invoked with + * the status of the transfer and the provided abstract parameter. + * Furthermore, the functions receives a slave parameter that can + * be used to connect multiple slaves, each with its individual + * chip select line. + * + * The interface also provides functionality to change the SPI + * baud rate. An additional abort method is used to cancel the + * ongoing transfer. + * + * **GPIO interface:** + * + * The GPIO part of the S2PI interface has two distinct concerns: + * + * First, the GPIO interface handles the measurement finished interrupt + * from the device. When the device invokes the interrupt, it pulls + * the interrupt line to low. Thus the interrupt must trigger when + * a transition from high to low occurs on the interrupt line. + * + * The module simply invokes a callback when this interrupt occurs. + * The #S2PI_SetIrqCallback method is used to install the callback + * for a specified slave. Each slave will have its own interrupt + * line. An additional callback parameter can be set that would be + * passed to the callback function. + * + * In addition to the interrupt, all SPI pins need to be accessible + * as GPIO pins through this interface. This is required to read + * the EEPROM memory on the device hat is connected to the SPI + * pins but requires a different protocol that is not compatible + * to any standard SPI interface. Therefore, the interface provides + * the possibility to switch to GPIO control mode that allows to + * emulate the EEPROM protocol via software bit banging. + * + * Two methods are provided to switch forth and back between SPI + * and GPIO control. In GPIO mode, several functions are used to + * read and write the individual GPIO pins. + * + * Note that the GPIO mode is only required to readout the EEPROM + * upon initialization of the device, i.e. during execution of the + * #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used + * during measurements. + * + * + * @addtogroup argus_s2pi + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" + +/*!*************************************************************************** + * @brief S2PI layer callback function type for the SPI transfer completed event. + * + * @param status The \link #status_t status\endlink of the completed + * transfer (#STATUS_OK on success). + * + * @param param The provided (optional, can be null) callback parameter. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +typedef status_t (*s2pi_callback_t)(status_t status, void *param); + +/*!*************************************************************************** + * @brief S2PI layer callback function type for the GPIO interrupt event. + * + * @param param The provided (optional, can be null) callback parameter. + *****************************************************************************/ +typedef void (*s2pi_irq_callback_t)(void *param); + +/*! The S2PI slave identifier. Basically an user defined enumerable type that + * can be used to identify the slave within the SPI module. */ +typedef int32_t s2pi_slave_t; + +/*! The enumeration of S2PI pins. */ +typedef enum { + /*! The SPI clock pin. */ + S2PI_CLK, + + /*! The SPI chip select pin. */ + S2PI_CS, + + /*! The SPI MOSI pin. */ + S2PI_MOSI, + + /*! The SPI MISO pin. */ + S2PI_MISO, + + /*! The IRQ pin. */ + S2PI_IRQ + +} s2pi_pin_t; + + +/*!*************************************************************************** + * @brief Returns the status of the SPI module. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. + * - #STATUS_BUSY: An SPI transfer is in progress. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. + *****************************************************************************/ +status_t S2PI_GetStatus(void); + +/*!*************************************************************************** + * @brief Transfers a single SPI frame asynchronously. + * + * @details Transfers a single SPI frame in asynchronous manner. The Tx data + * buffer is written to the device via the MOSI line. + * Optionally, the data on the MISO line is written to the provided + * Rx data buffer. If null, the read data is dismissed. Note that + * Rx and Tx buffer can be identical. I.e. the same buffer is used + * for writing and reading data. First, a byte is transmitted and + * the received byte overwrites the previously send value. + * + * The transfer of a single frame requires to not toggle the chip + * select line to high in between the data frame. The maximum + * number of bytes transfered in a single SPI transfer is given by + * the data value register of the device, which is 396 data bytes + * plus a single address byte: 397 bytes. + * + * An optional callback is invoked when the asynchronous transfer + * is finished. If the \p callback parameter is a null pointer, + * no callback is provided. Note that the provided buffer must not + * change while the transfer is ongoing. + * + * Use the slave parameter to determine the corresponding slave via the + * given chip select line. + * + * Usually, two distinct interrupts are required to handle the RX and + * TX ready events. The callback must be invoked from whichever + * interrupt comes after the SPI transfer has been finished. Note + * that new SPI transfers are invoked from within the callback function + * (i.e. from within the interrupt service routine of same priority). + * + * @param slave The specified S2PI slave. + * @param txData The 8-bit values to write to the SPI bus MOSI line. + * @param rxData The 8-bit values received from the SPI bus MISO line + * (pass a null pointer if the data don't need to be read). + * @param frameSize The number of 8-bit values to be sent/received. + * @param callback A callback function to be invoked when the transfer is + * finished. Pass a null pointer if no callback is required. + * @param callbackData A pointer to a state that will be passed to the + * callback. Pass a null pointer if not used. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully invoked the transfer. + * - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + * - #STATUS_BUSY: An SPI transfer is already in progress. The + * transfer was not started. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer + * was not started. + *****************************************************************************/ +status_t S2PI_TransferFrame(s2pi_slave_t slave, + uint8_t const *txData, + uint8_t *rxData, + size_t frameSize, + s2pi_callback_t callback, + void *callbackData); + +/*!*************************************************************************** + * @brief Terminates a currently ongoing asynchronous SPI transfer. + * + * @details When a callback is set for the current ongoing activity, it is + * invoked with the #ERROR_ABORTED error byte. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_Abort(void); + +/*!*************************************************************************** + * @brief Set a callback for the GPIO IRQ for a specified S2PI slave. + * + * @param slave The specified S2PI slave. + * @param callback A callback function to be invoked when the specified + * S2PI slave IRQ occurs. Pass a null pointer to disable + * the callback. + * @param callbackData A pointer to a state that will be passed to the + * callback. Pass a null pointer if not used. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully installation of the callback. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + *****************************************************************************/ +status_t S2PI_SetIrqCallback(s2pi_slave_t slave, + s2pi_irq_callback_t callback, + void *callbackData); + +/*!*************************************************************************** + * @brief Reads the current status of the IRQ pin. + * + * @details In order to keep a low priority for GPIO IRQs, the state of the + * IRQ pin must be read in order to reliable check for chip timeouts. + * + * The execution of the interrupt service routine for the data-ready + * interrupt from the corresponding GPIO pin might be delayed due to + * priority issues. The delayed execution might disable the timeout + * for the eye-safety checker too late causing false error messages. + * In order to overcome the issue, the state of the IRQ GPIO input + * pin is read before raising a timeout error in order to check if + * the device has already finished but the IRQ is still pending to be + * executed! + + * @param slave The specified S2PI slave. + * @return Returns 1U if the IRQ pin is high (IRQ not pending) and 0U if the + * devices pulls the pin to low state (IRQ pending). + *****************************************************************************/ +uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave); + +/*!*************************************************************************** + * @brief Cycles the chip select line. + * + * @details In order to cancel the integration on the ASIC, a fast toggling + * of the chip select pin of the corresponding SPI slave is required. + * Therefore, this function toggles the CS from high to low and back. + * The SPI instance for the specified S2PI slave must be idle, + * otherwise the status #STATUS_BUSY is returned. + * + * @param slave The specified S2PI slave. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_CycleCsPin(s2pi_slave_t slave); + + + +/*!***************************************************************************** + * @brief Captures the S2PI pins for GPIO usage. + * + * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the + * pins are configured for GPIO operation. The GPIO control must be + * release with the #S2PI_ReleaseGpioControl function in order to + * switch back to ordinary SPI functionality. + * + * @note This function is only called during device initialization! + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_CaptureGpioControl(void); + +/*!***************************************************************************** + * @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. + * + * @details The GPIO pins are configured for SPI operation and the GPIO mode is + * left. Must be called if the pins are captured for GPIO operation via + * the #S2PI_CaptureGpioControl function. + * + * @note This function is only called during device initialization! + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_ReleaseGpioControl(void); + +/*!***************************************************************************** + * @brief Writes the output for a specified SPI pin in GPIO mode. + * + * @details This function writes the value of an SPI pin if the SPI pins are + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * + * @note Since some GPIO peripherals switch the GPIO pins very fast a delay + * must be added after each GBIO access (i.e. right before returning + * from the #S2PI_WriteGpioPin method) in order to decrease the baud + * rate of the software EEPROM protocol. Increase the delay if timing + * issues occur while reading the EERPOM. For example: + * Delay = 10 µsec => Baud Rate < 100 kHz + * + * @note This function is only called during device initialization! + * + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to write (0 = low, 1 = high). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value); + +/*!***************************************************************************** + * @brief Reads the input from a specified SPI pin in GPIO mode. + * + * @details This function reads the value of an SPI pin if the SPI pins are + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * + * @note This function is only called during device initialization! + * + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value); +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif // ARGUS_S2PI_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h new file mode 100644 index 000000000000..80deb42016f9 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h @@ -0,0 +1,223 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required timer modules. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_TIMER_H +#define ARGUS_TIMER_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @defgroup argus_timer Timer: Hardware Timer Interface + * @ingroup argus_platform + * + * @brief Timer implementations for lifetime counting as well as periodic + * callback. + * + * @details The module provides an interface to the timing utilities that + * are required by the AFBR-S50 time-of-flight sensor API. + * + * Two essential features have to be provided by the user code: + * 1. Time Measurement Capability: In order to keep track of outgoing + * signals, the API needs to measure elapsed time. In order to + * provide optimum device performance, the granularity should be + * around 10 to 100 microseconds. + * 2. Periodic Callback: The API provides an automatic starting of + * measurement cycles at a fixed frame rate via a periodic + * interrupt timer. If this feature is not used, implementation + * of the periodic interrupts can be skipped. An weak default + * implementation is provide in the API. + * . + * + * The time measurement feature is simply implemented by the function + * #Timer_GetCounterValue. Whenever the function is called, the + * provided counter values must be written with the values obtained + * by the current time. + * + * The periodic interrupt timer is a simple callback interface. + * After installing the callback function pointer via #Timer_SetCallback, + * the timer can be started by setting interval via #Timer_SetInterval. + * From then, the callback is invoked periodically as the corresponding + * interval may specify. The timer is stopped by setting the interval + * to 0 using the #Timer_SetInterval function. The interval can be + * updated at any time by updating the interval via the #Timer_SetInterval + * function. To any of these functions, an abstract parameter pointer + * must be passed. This parameter is passed back to the callback any + * time it is invoked. + * + * In order to provide the usage of multiple devices, an mechanism is + * introduced to allow the installation of multiple callback interval + * at the same time. Therefore, the abstract parameter pointer is used + * to identify the corresponding callback interval. For example, there + * are two callbacks for two intervals, t1 and t2, required. The user + * can start two timers by calling the #Timer_SetInterval method twice, + * but with an individual parameter pointer, ptr1 and ptr2, each: + * \code + * Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1 + * Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1 + * \endcode + * + * Note that the implemented timer module must therefore support + * as many different intervals as instances of the AFBR-S50 device are + * used. + * + * @addtogroup argus_timer + * @{ + *****************************************************************************/ + +#include "api/argus_def.h" + +/******************************************************************************* + * Lifetime Counter Timer Interface + ******************************************************************************/ + +/*!*************************************************************************** + * @brief Obtains the lifetime counter value from the timers. + * + * @details The function is required to get the current time relative to any + * point in time, e.g. the startup time. The returned values \p hct and + * \p lct are given in seconds and microseconds respectively. The current + * elapsed time since the reference time is then calculated from: + * + * t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec + * + * Note that the accuracy/granularity of the lifetime counter does + * not need to be 1 µsec. Usually, a granularity of approximately + * 100 µsec is sufficient. However, in case of very high frame rates + * (above 1000 frames per second), it is recommended to implement + * an even lower granularity (somewhere in the 10 µsec regime). + * + * It must be guaranteed, that each call of the #Timer_GetCounterValue + * function must provide a value that is greater or equal, but never lower, + * than the value returned from the previous call. + * + * A hardware based implementation of the lifetime counter functionality + * would be to chain two distinct timers such that counter 2 increases + * its value when counter 1 wraps to 0. The easiest way is to setup + * counter 1 to wrap exactly every second. Counter 1 would than count + * the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds + * (\p hct) value. A 16-bit counter is sufficient in case of counter 1 + * while counter 2 must be a 32-bit version. + * + * In case of a lack of available hardware timers, a software solution + * can be used that requires only a 16-bit timer. In a simple scenario, + * the timer is configured to wrap around every second and increase + * a software counter value in its interrupt service routine (triggered + * with the wrap around event) every time the wrap around occurs. + * + * + * @note The implementation of this function is mandatory for the correct + * execution of the API. + * + * @param hct A pointer to the high counter value bits representing current + * time in seconds. + * + * @param lct A pointer to the low counter value bits representing current + * time in microseconds. Range: 0, .., 999999 µsec + *****************************************************************************/ +void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct); + +/******************************************************************************* + * Periodic Interrupt Timer Interface + ******************************************************************************/ + +/*!*************************************************************************** + * @brief The callback function type for periodic interrupt timer. + * + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. + * + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + *****************************************************************************/ +typedef void (*timer_cb_t)(void *param); + +/*!*************************************************************************** + * @brief Installs an periodic timer callback function. + * + * @details Installs an periodic timer callback function that is invoked whenever + * an interval elapses. The callback is the same for any interval, + * however, the single intervals can be identified by the passed + * parameter. + * Passing a zero-pointer removes and disables the callback. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. + * + * @param f The timer callback function. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Timer_SetCallback(timer_cb_t f); + +/*!*************************************************************************** + * @brief Sets the timer interval for a specified callback parameter. + * + * @details Sets the callback interval for the specified parameter and starts + * the timer with a new interval. If there is already an interval with + * the given parameter, the timer is restarted with the given interval. + * If the same time interval as already set is passed, nothing happens. + * Passing a interval of 0 disables the timer. + * + * Note that a microsecond granularity for the timer interrupt period is + * not required. Usually a microseconds granularity is sufficient. + * The required granularity depends on the targeted frame rate, e.g. in + * case of more than 1 kHz measurement rate, a granularity of less than + * a microsecond is required to achieve the given frame rate. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. + * + * @param dt_microseconds The callback interval in microseconds. + * + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Timer_SetInterval(uint32_t dt_microseconds, void *param); + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif /* ARGUS_TIMER_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h new file mode 100644 index 000000000000..ae7422ada596 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h @@ -0,0 +1,525 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 FP_DEF_H +#define FP_DEF_H + +/*!*************************************************************************** + * @defgroup fixedpoint Fixed Point Math + * @ingroup argusutil + * @brief A basic math library for fixed point number in the Qx.y fomat. + * @details This module contains common fixed point type definitions as + * well as some basic math algorithms. All types are based on + * integer types. The number are defined with the Q number format. + * + * - For a description of the Q number format refer to: + * https://en.wikipedia.org/wiki/Q_(number_format) + * - Another resource for fixed point math in C might be found at + * http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491 + * . + * @warning This definitions are not portable and work only with + * little-endian systems! + * @addtogroup fixedpoint + * @{ + *****************************************************************************/ + +#include + +/******************************************************************************* + ***** Unsigned 8-Bit Values *********************************************** + ******************************************************************************/ + +/******************************************************************************* + ***** UQ6.2 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ6.2 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 6 integer and 2 fractional bits. + * - Range: 0 .. 63.75 + * - Granularity: 0.25 + *****************************************************************************/ +typedef uint8_t uq6_2_t; + +/*! Maximum value of UQ6.2 number format. */ +#define UQ6_2_MAX ((uq6_2_t)UINT8_MAX) + +/*! The 1/one/unity in UQ6.2 number format. */ +#define UQ6_2_ONE ((uq6_2_t)(1U<<2U)) + + + +/******************************************************************************* + ***** UQ4.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ4.4 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 4 integer and 4 fractional bits. + * - Range: 0 .. 15.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef uint8_t uq4_4_t; + +/*! Maximum value of UQ4.4 number format. */ +#define UQ4_4_MAX ((uq4_4_t)UINT8_MAX) + +/*! The 1/one/unity in UQ4.4 number format. */ +#define UQ4_4_ONE ((uq4_4_t)(1U<<4U)) + + + +/******************************************************************************* + ***** UQ2.6 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ2.6 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 2 integer and 6 fractional bits. + * - Range: 0 .. 3.984375 + * - Granularity: 0.015625 + *****************************************************************************/ +typedef uint8_t uq2_6_t; + +/*! Maximum value of UQ2.6 number format. */ +#define UQ2_6_MAX ((uq2_6_t)UINT8_MAX) + +/*! The 1/one/unity in UQ2.6 number format. */ +#define UQ2_6_ONE ((uq2_6_t)(1U<<6U)) + + + +/******************************************************************************* + ***** UQ1.7 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ1.7 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 1.9921875 + * - Granularity: 0.0078125 + *****************************************************************************/ +typedef uint8_t uq1_7_t; + +/*! Maximum value of UQ1.7 number format. */ +#define UQ1_7_MAX ((uq1_7_t)UINT8_MAX) + +/*! The 1/one/unity in UQ1.7 number format. */ +#define UQ1_7_ONE ((uq1_7_t)(1U<<7U)) + + + +/******************************************************************************* + ***** UQ0.8 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ0.8 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 0.99609375 + * - Granularity: 0.00390625 + *****************************************************************************/ +typedef uint8_t uq0_8_t; + +/*! Maximum value of UQ0.8 number format. */ +#define UQ0_8_MAX ((uq0_8_t)UINT8_MAX) + + + +/******************************************************************************* + ***** Signed 8-Bit Values ************************************************* + ******************************************************************************/ + +/******************************************************************************* + ***** Q3.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q3.4 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 3 integer and 4 fractional bits. + * - Range: -8 ... 7.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef int8_t q3_4_t; + +/*! Minimum value of Q3.4 number format. */ +#define Q3_4_MIN ((q3_4_t)INT8_MIN) + +/*! Maximum value of Q3.4 number format. */ +#define Q3_4_MAX ((q3_4_t)INT8_MAX) + +/*! The 1/one/unity in UQ3.4 number format. */ +#define UQ3_4_ONE ((q3_4_t)(1 << 4)) + + +/******************************************************************************* + ***** Q1.6 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q1.6 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 1 integer and 6 fractional bits. + * - Range: -2 ... 1.984375 + * - Granularity: 0.015625 + *****************************************************************************/ +typedef int8_t q1_6_t; + +/*! Minimum value of Q1_6 number format. */ +#define Q1_6_MIN ((q1_6_t)INT8_MIN) + +/*! Maximum value of Q1.6 number format. */ +#define Q1_6_MAX ((q1_6_t)INT8_MAX) + +/*! The 1/one/unity in UQ1.6 number format. */ +#define UQ1_6_ONE ((q1_6_t)(1 << 4)) + + +/******************************************************************************* + ***** Unsigned 16-Bit Values ********************************************** + ******************************************************************************/ + +/******************************************************************************* + ***** UQ12.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ12.4 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 12 integer and 4 fractional bits. + * - Range: 0 ... 4095.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef uint16_t uq12_4_t; + +/*! Maximum value of UQ12.4 number format. */ +#define UQ12_4_MAX ((uq12_4_t)UINT16_MAX) + +/*! The 1/one/unity in UQ12.4 number format. */ +#define UQ12_4_ONE ((uq12_4_t)(1U<<4U)) + + + +/******************************************************************************* + ***** UQ10.6 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ10.6 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 10 integer and 6 fractional bits. + * - Range: 0 ... 1023.984375 + * - Granularity: 0.015625 + *****************************************************************************/ +typedef uint16_t uq10_6_t; + +/*! Maximum value of UQ10.6 number format. */ +#define UQ10_6_MAX ((uq10_6_t)UINT16_MAX) + +/*! The 1/one/unity in UQ10.6 number format. */ +#define UQ10_6_ONE ((uq10_6_t)(1U << 6U)) + + + +/******************************************************************************* + ***** UQ1.15 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ1.15 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 1 integer and 15 fractional bits. + * - Range: 0 .. 1.999969 + * - Granularity: 0.000031 + *****************************************************************************/ +typedef uint16_t uq1_15_t; + +/*! Maximum value of UQ1.15 number format. */ +#define UQ1_15_MAX ((uq1_15_t)UINT16_MAX) + +/*! The 1/one/unity in UQ1.15 number format. */ +#define UQ1_15_ONE ((uq1_15_t)(1U << 15U)) + + + +/******************************************************************************* + ***** UQ0.16 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ0.16 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 0 integer and 16 fractional bits. + * - Range: 0 .. 0.9999847412109375 + * - Granularity: 1.52587890625e-5 + *****************************************************************************/ +typedef uint16_t uq0_16_t; + +/*! Maximum value of UQ0.16 number format. */ +#define UQ0_16_MAX ((uq0_16_t)UINT16_MAX) + + + +/******************************************************************************* + ***** Signed 16-Bit Values ************************************************ + ******************************************************************************/ + +/******************************************************************************* + ***** Q11.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q11.4 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 11 integer and 4 fractional bits. + * - Range: -2048 ... 2047.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef int16_t q11_4_t; + +/*! Minimum value of Q11.4 number format. */ +#define Q11_4_MIN ((q11_4_t)INT16_MIN) + +/*! Maximum value of Q11.4 number format. */ +#define Q11_4_MAX ((q11_4_t)INT16_MAX) + +/*! The 1/one/unity in Q11.4 number format. */ +#define UQ11_4_ONE ((q11_4_t)(1 << 4)) + + + +/******************************************************************************* + ***** Q7.8 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q7.8 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 7 integer and 8 fractional bits. + * - Range: -128 .. 127.99609375 + * - Granularity: 0.00390625 + *****************************************************************************/ +typedef int16_t q7_8_t; + +/*! Minimum value of Q7.8 number format. */ +#define Q7_8_MIN ((q7_8_t)INT16_MIN) + +/*! Maximum value of Q7.8 number format. */ +#define Q7_8_MAX ((q7_8_t)INT16_MAX) + +/*! The 1/one/unity in Q7.8 number format. */ +#define Q7_8_ONE ((q7_8_t)(1 << 8)) + + + +/******************************************************************************* + ***** Q3.12 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q3.12 + * @details A signed fixed point number format based on the 16-bit integer + * type with 3 integer and 12 fractional bits. + * - Range: -8 .. 7.99975586 + * - Granularity: 0.00024414 + *****************************************************************************/ +typedef int16_t q3_12_t; + +/*! Minimum value of Q3.12 number format. */ +#define Q3_12_MIN ((q3_12_t)INT16_MIN) + +/*! Maximum value of Q3.12 number format. */ +#define Q3_12_MAX ((q3_12_t)INT16_MAX) + +/*! The 1/one/unity in Q3.12 number format. */ +#define Q3_12_ONE ((q3_12_t)(1 << 12)) + + + +/******************************************************************************* + ***** Q0.15 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q0.15 + * @details A signed fixed point number format based on the 16-bit integer + * type with 0 integer and 15 fractional bits. + * - Range: -1 .. 0.999969482 + * - Granularity: 0.000030518 + *****************************************************************************/ +typedef int16_t q0_15_t; + +/*! Minimum value of Q0.15 number format. */ +#define Q0_15_MIN ((q0_15_t)INT16_MIN) + +/*! Maximum value of Q0.15 number format. */ +#define Q0_15_MAX ((q0_15_t)INT16_MAX) + + + +/******************************************************************************* + ***** Unsigned 32-Bit Values ********************************************** + ******************************************************************************/ + +/******************************************************************************* + ***** UQ28.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ28.4 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 28 integer and 4 fractional bits. + * - Range: 0 ... 268435455.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef uint32_t uq28_4_t; + +/*! Maximum value of UQ28.4 number format. */ +#define UQ28_4_MAX ((uq28_4_t)UINT32_MAX) + +/*! The 1/one/unity in UQ28.4 number format. */ +#define UQ28_4_ONE ((uq28_4_t)(1U<<4U)) + + + +/******************************************************************************* + ***** UQ16.16 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ16.16 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 16 integer and 16 fractional bits. + * - Range: 0 ... 65535.999984741 + * - Granularity: 0.000015259 + *****************************************************************************/ +typedef uint32_t uq16_16_t; + +/*! Maximum value of UQ16.16 number format. */ +#define UQ16_16_MAX ((uq16_16_t)UINT32_MAX) + +/*! The 1/one/unity in UQ16.16 number format. */ +#define UQ16_16_ONE ((uq16_16_t)(1U << 16U)) + +/*! Euler's number, e, in UQ16.16 format. */ +#define UQ16_16_E (0x2B7E1U) + + + +/******************************************************************************* + ***** UQ10.22 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Unsigned fixed point number: UQ10.22 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 10 integer and 22 fractional bits. + * - Range: 0 ... 1023.99999976158 + * - Granularity: 2.38418579101562E-07 + *****************************************************************************/ +typedef uint32_t uq10_22_t; + +/*! Maximum value of UQ10.22 number format. */ +#define UQ10_22_MAX ((uq10_22_t)UINT32_MAX) + +/*! The 1/one/unity in UQ10.22 number format. */ +#define UQ10_22_ONE ((uq10_22_t)(1U << 22U)) + + + +/******************************************************************************* + ***** Signed 32-Bit Values ************************************************ + ******************************************************************************/ + +/******************************************************************************* + ***** Q27.4 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q27.4 + * @details A signed fixed point number format based on the 32-bit signed + * integer type with 27 integer and 4 fractional bits. + * - Range: -134217728 ... 134217727.9375 + * - Granularity: 0.0625 + *****************************************************************************/ +typedef int32_t q27_4_t; + +/*! Minimum value of Q27.4 number format. */ +#define Q27_4_MIN ((q27_4_t)INT32_MIN) + +/*! Maximum value of Q27.4 number format. */ +#define Q27_4_MAX ((q27_4_t)INT32_MAX) + +/*! The 1/one/unity in UQ27.4 number format. */ +#define UQ27_4_ONE ((q27_4_t)(1 << 4)) + + + +/******************************************************************************* + ***** Q15.16 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q15.16 + * @details A signed fixed point number format based on the 32-bit integer + * type with 15 integer and 16 fractional bits. + * - Range: -32768 .. 32767.99998 + * - Granularity: 1.52588E-05 + *****************************************************************************/ +typedef int32_t q15_16_t; + +/*! Minimum value of Q15.16 number format. */ +#define Q15_16_MIN ((q15_16_t)INT32_MIN) + +/*! Maximum value of Q15.16 number format. */ +#define Q15_16_MAX ((q15_16_t)INT32_MAX) + +/*! The 1/one/unity in Q15.16 number format. */ +#define Q15_16_ONE ((q15_16_t)(1 << 16)) + + + +/******************************************************************************* + ***** Q9.22 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q9.22 + * @details A signed fixed point number format based on the 32-bit integer + * type with 9 integer and 22 fractional bits. + * - Range: -512 ... 511.9999998 + * - Granularity: 2.38418579101562E-07 + *****************************************************************************/ +typedef int32_t q9_22_t; + +/*! Minimum value of Q9.22 number format. */ +#define Q9_22_MIN ((q9_22_t)INT32_MIN) + +/*! Maximum value of Q9.22 number format. */ +#define Q9_22_MAX ((q9_22_t)INT32_MAX) + +/*! The 1/one/unity in Q9.22 number format. */ +#define Q9_22_ONE ((q9_22_t)(1 << 22)) + + + +/*! @} */ +#endif /* FP_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h new file mode 100644 index 000000000000..9c5f930351de --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h @@ -0,0 +1,290 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides utility functions for timing necessities. + * + * @copyright + * + * Copyright (c) 2021, Broadcom Inc + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 TIME_H +#define TIME_H + +/*!*************************************************************************** + * @defgroup time Time Utility + * @ingroup argusutil + * @brief Timer utilities for time measurement duties. + * @details This module provides time measurement utility functions like + * delay or time measurement methods, or time math functions. + * @addtogroup time + * @{ + *****************************************************************************/ + +#include +#include + +/*!*************************************************************************** + * @brief A data structure to represent current time. + * + * @details Value is obtained from the PIT time which must be configured as + * lifetime counter. + *****************************************************************************/ +typedef struct { + /*! Seconds. */ + uint32_t sec; + + /*! Microseconds. */ + uint32_t usec; + +} ltc_t; + +/*!*************************************************************************** + * @brief Obtains the elapsed time since MCU startup. + * @param t_now returned current time + *****************************************************************************/ +void Time_GetNow(ltc_t *t_now); + +/*!*************************************************************************** + * @brief Obtains the elapsed microseconds since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @param - + * @return Elapsed microseconds since MCU startup as uint32_t. + *****************************************************************************/ +uint32_t Time_GetNowUSec(void); + +/*!*************************************************************************** + * @brief Obtains the elapsed milliseconds since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @param - + * @return Elapsed milliseconds since MCU startup as uint32_t. + *****************************************************************************/ +uint32_t Time_GetNowMSec(void); + +/*!*************************************************************************** + * @brief Obtains the elapsed seconds since MCU startup. + * @param - + * @return Elapsed seconds since MCU startup as uint32_t. + *****************************************************************************/ +uint32_t Time_GetNowSec(void); + +/*!*************************************************************************** + * @brief Obtains the elapsed time since a given time point. + * @param t_elapsed Returns the elapsed time since t_start. + * @param t_start Start time point. + *****************************************************************************/ +void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start); + +/*!*************************************************************************** + * @brief Obtains the elapsed microseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed microseconds since t_start as uint32_t. + *****************************************************************************/ +uint32_t Time_GetElapsedUSec(ltc_t const *t_start); + +/*!*************************************************************************** + * @brief Obtains the elapsed milliseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed milliseconds since t_start as uint32_t. + *****************************************************************************/ +uint32_t Time_GetElapsedMSec(ltc_t const *t_start); + +/*!*************************************************************************** + * @brief Obtains the elapsed seconds since a given time point. + * @param t_start Start time point. + * @return Elapsed seconds since t_start as uint32_t. + *****************************************************************************/ +uint32_t Time_GetElapsedSec(ltc_t const *t_start); + +/*!*************************************************************************** + * @brief Obtains the time difference between two given time points. + * @details Result is defined as t_diff = t_end - t_start. + * Note: since no negative time differences are supported, t_end has + * to be later/larger than t_start. Otherwise, the result won't be + * a negative time span but given by max value. + * @param t_diff Returned time difference. + * @param t_start Start time point. + * @param t_end End time point. + *****************************************************************************/ +void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end); + +/*!*************************************************************************** + * @brief Obtains the time difference between two given time points in + * microseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow such that to large + * values are limited by 0xFFFFFFFF µs. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in microseconds. + *****************************************************************************/ +uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end); + +/*!*************************************************************************** + * @brief Obtains the time difference between two given time points in + * milliseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in milliseconds. + *****************************************************************************/ +uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end); + +/*!*************************************************************************** + * @brief Obtains the time difference between two given time points in + * seconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in seconds. + *****************************************************************************/ +uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end); + +/*!*************************************************************************** + * @brief Time delay for a given time period. + * @param dt Delay time. + *****************************************************************************/ +void Time_Delay(ltc_t const *dt); + +/*!*************************************************************************** + * @brief Time delay for a given time period in microseconds. + * @param dt_usec Delay time in microseconds. + *****************************************************************************/ +void Time_DelayUSec(uint32_t dt_usec); + +/*!*************************************************************************** + * @brief Time delay for a given time period in milliseconds. + * @param dt_msec Delay time in milliseconds. + *****************************************************************************/ +void Time_DelayMSec(uint32_t dt_msec); + +/*!*************************************************************************** + * @brief Time delay for a given time period in seconds. + * @param dt_sec Delay time in seconds. + *****************************************************************************/ +void Time_DelaySec(uint32_t dt_sec); + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout Timeout period. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout); + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_usec Timeout period in microseconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec); + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_msec Timeout period in milliseconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec); + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_sec Timeout period in seconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec); + +/*!*************************************************************************** + * @brief Adds two ltc_t values. + * @details Result is defined as t = t1 + t2. Results are wrapped around at + * maximum values just like integers. + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2 2nd operand. + *****************************************************************************/ +void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2); + +/*!*************************************************************************** + * @brief Adds a given time in microseconds to an ltc_t value. + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_usec 2nd operand in microseconds. + *****************************************************************************/ +void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec); + +/*!*************************************************************************** + * @brief Adds a given time in milliseconds to an ltc_t value. + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_msec 2nd operand in milliseconds. + *****************************************************************************/ +void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec); + +/*!*************************************************************************** + * @brief Adds a given time in seconds to an ltc_t value. + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_sec 2nd operand in seconds. + *****************************************************************************/ +void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec); + +/*!*************************************************************************** + * @brief Converts ltc_t to microseconds (uint32_t). + * @param t Input ltc_t struct. + * @return Time value in microseconds. + *****************************************************************************/ +uint32_t Time_ToUSec(ltc_t const *t); + +/*!*************************************************************************** + * @brief Converts ltc_t to milliseconds (uint32_t). + * @param t Input ltc_t struct. + * @return Time value in milliseconds. + *****************************************************************************/ +uint32_t Time_ToMSec(ltc_t const *t); + +/*!*************************************************************************** + * @brief Converts ltc_t to seconds (uint32_t). + * @param t Input ltc_t struct. + * @return Time value in seconds. + *****************************************************************************/ +uint32_t Time_ToSec(ltc_t const *t); + +/*! @} */ +#endif /* TIME_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Kconfig b/src/drivers/distance_sensor/broadcom/afbrs50/Kconfig new file mode 100644 index 000000000000..2b88b4633d46 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50 + bool "AFBRS50" + default n + ---help--- + Enable support for AFBRS50 diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a new file mode 100644 index 000000000000..c646ab311623 Binary files /dev/null and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a new file mode 100644 index 000000000000..905b2fa66d34 Binary files /dev/null and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c new file mode 100644 index 000000000000..8b877fa1d473 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c @@ -0,0 +1,1398 @@ +/*************************************************************************//** + * @file argus_hal_test.c + * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * + * @copyright + * + * Copyright (c) 2021, Broadcom, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 Files + ******************************************************************************/ +#include "argus_hal_test.h" + +#include "platform/argus_print.h" +#include "platform/argus_s2pi.h" +#include "platform/argus_timer.h" +#include "platform/argus_nvm.h" +#include "platform/argus_irq.h" + +#include +#include + +/******************************************************************************* + * Definitions + ******************************************************************************/ + +/*! An error log message via print(); */ +#define error_log(fmt, ...) print("ERROR: " fmt "\n", ##__VA_ARGS__) + +/******************************************************************************* + * Prototypes + ******************************************************************************/ +static status_t TimerPlausibilityTest(void); +static status_t TimerWraparoundTest(void); +static status_t SpiConnectionTest(s2pi_slave_t slave); +static status_t SpiInterruptTest(s2pi_slave_t slave); +static status_t GpioModeTest(s2pi_slave_t slave); +static status_t TimerTest(s2pi_slave_t slave); +static status_t PITTest(void); + +static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct); +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size); +static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim); +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples); +static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms); +static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom); +static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *RcoTrim); +static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples); +static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n); + +static void PIT_Callback(void *param); +static void DataReadyCallback(void *param); + +/// @cond EXTERN +extern uint32_t EEPROM_ReadChipId(uint8_t const *eeprom); +extern argus_module_version_t EEPROM_ReadModule(uint8_t const *eeprom); +extern status_t EEPROM_Read(s2pi_slave_t slave, uint8_t address, uint8_t *data); +extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); +/// @endcond + +/****************************************************************************** + * Variables + ******************************************************************************/ + +/******************************************************************************* + * Code + ******************************************************************************/ + +status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave) +{ + status_t status = STATUS_OK; + + PX4_INFO_RAW("########################################################\n"); + PX4_INFO_RAW("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); + PX4_INFO_RAW("########################################################\n\n"); + + PX4_INFO_RAW("1 > Timer Plausibility Test\n"); + status = TimerPlausibilityTest(); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("1 > PASS\n\n"); + + PX4_INFO_RAW("2 > Timer Wraparound Test\n"); + status = TimerWraparoundTest(); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("2 > PASS\n\n"); + + PX4_INFO_RAW("3 > SPI Connection Test\n"); + status = SpiConnectionTest(spi_slave); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("3 > PASS\n\n"); + + PX4_INFO_RAW("4 > SPI Interrupt Test\n"); + status = SpiInterruptTest(spi_slave); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("4 > PASS\n\n"); + + PX4_INFO_RAW("5 > GPIO Mode Test\n"); + status = GpioModeTest(spi_slave); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("5 > PASS\n\n"); + + PX4_INFO_RAW("6 > Lifetime Counter Timer (LTC) Test\n"); + status = TimerTest(spi_slave); + + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("6 > PASS\n\n"); + + PX4_INFO_RAW("7 > Periodic Interrupt Timer (PIT) Test\n"); + status = PITTest(); + + if (status == ERROR_NOT_IMPLEMENTED) { + PX4_INFO_RAW("7 > SKIPPED (PIT is not implemented)\n\n"); + + } else { + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("7 > PASS\n\n"); + } + + +summary: + PX4_INFO_RAW("########################################################\n"); + + if (status != STATUS_OK) { + PX4_INFO_RAW("# FAIL: HAL Verification Test finished with error %d!\n", (int)status); + + } else { + PX4_INFO_RAW("# PASS: HAL Verification Test finished successfully!\n"); + } + + PX4_INFO_RAW("########################################################\n\n"); + return status; +} + +/*!*************************************************************************** + * @brief Checks the validity of timer counter values. + * + * @details This verifies that the counter values returned from the + * #Timer_GetCounterValue function are valid. This means, the low + * counter value \p lct is within 0 and 999999 µs. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). + *****************************************************************************/ +static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) +{ + if (lct > 999999) { + PX4_INFO_RAW("Timer plausibility check:\n" + "The parameter \"lct\" of Timer_GetCounterValue() must always " + "be within 0 and 999999.\n" + "Current Values: hct = %d, lct = %d", (uint)hct, (uint)lct); + return ERROR_FAIL; + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Plausibility Test for Timer HAL Implementation. + * + * @details Rudimentary tests the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. + * + * @warning If using an ultra-fast processor with a rather low timer granularity, + * the test may fail! In this case, it could help to increase the delay + * by increasing the for-loop exit criteria. + * + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). + *****************************************************************************/ +static status_t TimerPlausibilityTest(void) +{ + uint32_t hct0 = 0; + uint32_t lct0 = 0; + uint32_t hct1 = 0; + uint32_t lct1 = 0; + + /* Get some start values */ + Timer_GetCounterValue(&hct0, &lct0); + + /* Check max value is not exceeded for LCT timer (us) */ + status_t status = CheckTimerCounterValues(hct0, lct0); + + if (status < STATUS_OK) { return status; } + + /* Adding a delay. Depending on MCU speed, this takes any time. + * However, the Timer should be able to solve this on any MCU. */ + for (volatile uint32_t i = 0; i < 100000; ++i) { __asm("nop"); } + + /* Get new timer value and verify some time has elapsed. */ + Timer_GetCounterValue(&hct1, &lct1); + + /* Check max value is not exceeded for LCT timer (us) */ + status = CheckTimerCounterValues(hct1, lct1); + + if (status < STATUS_OK) { return status; } + + /* Either the hct value must have been increased or the lct value if the hct + * value is still the same. */ + if (!((hct1 > hct0) || ((hct1 == hct0) && (lct1 > lct0)))) { + PX4_INFO_RAW("Timer plausibility check: the elapsed time could not be " + "measured with the Timer_GetCounterValue() function; no time " + "has elapsed!\n" + "The delay was induced by the following code:\n" + "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + return ERROR_FAIL; + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Wraparound Test for the Timer HAL Implementation. + * + * @details The LTC values must wrap from 999999 µs to 0 µs and increase the + * seconds counter accordingly. This test verifies the correct wrapping + * by consecutively calling the #Timer_GetCounterValue function until + * at least 2 wraparound events have been occurred. + * + * @note This test requires the timer to basically run and return ascending + * values. Also, if the timer is too slow, this may take very long! + * Usually, the test takes 2 seconds, since 2 wraparound events are + * verified. + * + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). + *****************************************************************************/ +static status_t TimerWraparoundTest(void) +{ + /* Test parameter configuration: *****************************************/ + const int8_t n = 2; // The number of wraparounds to test. + /*************************************************************************/ + + uint32_t hct0 = 0; + uint32_t lct0 = 0; + uint32_t hct1 = 0; + uint32_t lct1 = 0; + + /* Get some start values. */ + Timer_GetCounterValue(&hct0, &lct0); + + /* Check max value is not exceeded for LCT timer (us) */ + status_t status = CheckTimerCounterValues(hct0, lct0); + + if (status < STATUS_OK) { return status; } + + /* Set end after 2 seconds, i.e. 2 wrap around events. */ + uint32_t hct2 = hct0 + n; + uint32_t lct2 = lct0; + + px4_usleep(20000); + + /* Periodically read timer values. From previous tests we + * already know the timer value is increasing. */ + while (hct0 < hct2 || lct0 < lct2) { + /* add counter a , which is increasing by +1, 1000000 or 1000, + * different MCU different times get stuck for hard code value */ + Timer_GetCounterValue(&hct1, &lct1); + + /* Check max value is not exceeded for LCT timer (us) */ + status = CheckTimerCounterValues(hct0, lct0); + + if (status < STATUS_OK) { return status; } + + /* Testing if calls to Timer_GetCounterValue are equal or increasing. + * Also testing if wraparound is correctly handled. + * Assumption here is that two sequential calls to the get functions are + * only a few µs appart! I.e. if hct wraps, the new lct must be smaller + * than previous one. */ + if (!(((hct1 == hct0 + 1) && (lct1 < lct0)) + || ((hct1 == hct0) && (lct1 >= lct0)))) { + PX4_INFO_RAW("Timer plausibility check: the wraparound of \"lct\" or " + "\"hct\" parameters of the Timer_GetCounterValue() " + "function was not handled correctly!\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + return ERROR_FAIL; + } + + hct0 = hct1; + lct0 = lct1; + + px4_usleep(20000); + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Helper function for transfer data to SPI in blocking mode. + * + * @details Calls the #S2PI_TransferFrame function and waits until the transfer + * has been finished by checking the #S2PI_GetStatus return code to + * become #STATUS_IDLE (or #STATUS_OK). + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param data The data array to be transfered. + * @param size The size of the data array to be transfered. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 100; // The transfer timeout in ms. + /*************************************************************************/ + + status_t status = S2PI_TransferFrame(slave, data, data, size, 0, 0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI transfer failed! The call to S2PI_TransferFrame " + "yielded error code: %d", (int)status); + return status; + } + + /* Wait until the transfer is finished using a timeout. + * Note: this already utilizes the timer HAL. So we might + * need to test the timer before the SPI connection test. */ + ltc_t start; + Time_GetNow(&start); + + do { + status = S2PI_GetStatus(); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI transfer failed! The call to S2PI_GetStatus " + "yielded error code: %d", (int)status); + S2PI_Abort(); + return status; + } + + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + PX4_INFO_RAW("SPI transfer failed! The operation did not finished " + "within %u ms. This may also be caused by an invalid " + "timer implementation!", (uint)timeout_ms); + return ERROR_TIMEOUT; + } + } while (status == STATUS_BUSY); + + return status; +} + +/*!*************************************************************************** + * @brief SPI Connection Test for S2PI HAL Implementation. + * + * @details This test verifies the basic functionality of the SPI interface. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t SpiConnectionTest(s2pi_slave_t slave) +{ + status_t status = STATUS_OK; + uint8_t data[17U] = { 0 }; + + /* Transfer a pattern to the register */ + data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data[i] = i; } + + status = SPITransferSync(slave, data, 17U); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI connection test failed!"); + return status; + } + + /* Clear the laser pattern and read back previous values. */ + data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data[i] = 0; } + + status = SPITransferSync(slave, data, 17U); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI connection test failed!"); + return status; + } + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data[i] != i) { + PX4_INFO_RAW("SPI connection test failed!\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + + +/*!*************************************************************************** + * @brief The data ready callback invoked by the API. + * + * @details The callback is invoked by the API when the device GPIO IRQ is + * pending after a measurement has been executed and data is ready to + * be read from the device. + * + * @param param The abstract pointer to the boolean value that determines if + * the callback is invoked. + *****************************************************************************/ +static void DataReadyCallback(void *param) +{ + IRQ_LOCK(); + *((bool *) param) = true; + IRQ_UNLOCK(); +} + +/*!*************************************************************************** + * @brief Configures the device with a bare minimum setup to run the tests. + * + * @details This function applies a number of configuration values to the + * device, such that a pseudo measurement w/o laser output can be + * performed. + * + * A \p rcoTrim parameter can be passed to adjust the actual clock + * setup. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcoTrim The RCO Trimming value added to the nominal RCO register + * value. Pass 0 if no fine tuning is required. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the SPI operation did not finished within a + * specified time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) +{ + /* Setup Device and Trigger Measurement. */ + uint16_t v = 0x0010U | (((34 + rcoTrim) & 0x3F) << 6); + uint8_t d1[] = { 0x14, v >> 8, v & 0xFF, 0x21 }; + status_t status = SPITransferSync(slave, d1, sizeof(d1)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d2[] = { 0x16, 0x7F, 0xFF, 0x7F, 0xE9 }; + status = SPITransferSync(slave, d2, sizeof(d2)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d3[] = { 0x18, 0x00, 0x00, 0x03 }; + status = SPITransferSync(slave, d3, sizeof(d3)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d4[] = { 0x10, 0x12 }; + status = SPITransferSync(slave, d4, sizeof(d4)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d5[] = { 0x12, 0x00, 0x2B }; + status = SPITransferSync(slave, d5, sizeof(d5)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d6[] = { 0x08, 0x04, 0x84, 0x10 }; + status = SPITransferSync(slave, d6, sizeof(d6)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d7[] = { 0x0A, 0xFE, 0x51, 0x0F, 0x05 }; + status = SPITransferSync(slave, d7, sizeof(d7)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d8[] = { 0x0C, 0x00, 0x00, 0x00 }; + status = SPITransferSync(slave, d8, sizeof(d8)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d9[] = { 0x1E, 0x00, 0x00, 0x00 }; + status = SPITransferSync(slave, d9, sizeof(d9)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d10[] = { 0x20, 0x01, 0xFF, 0xFF }; + status = SPITransferSync(slave, d10, sizeof(d10)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + uint8_t d11[] = { 0x22, 0xFF, 0xFF, 0x04 }; + status = SPITransferSync(slave, d11, sizeof(d11)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Device configuration failed!"); + return status; + } + + return status; +} + +/*!*************************************************************************** + * @brief Triggers a measurement on the device with specified sample count. + * + * @details The function triggers a measurement cycle on the device. A + * \p sample count can be specified to setup individual number of + * digital averaging. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) +{ + // samples is zero based, i.e. writing 0 yields 1 sample + samples = samples > 0 ? samples - 1 : samples; + uint16_t v = 0x8000U | ((samples & 0x03FFU) << 5U); + uint8_t d[] = { 0x1C, v >> 8, v & 0xFFU }; + status_t status = SPITransferSync(slave, d, sizeof(d)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Trigger measurement failed!"); + return status; + } + + return status; +} + +/*!*************************************************************************** + * @brief Waits for the data ready interrupt to be pending. + * + * @details The function polls the current interrupt pending state of the data + * ready interrupt from the device, i.e. reads the IRQ GPIO pin until + * it is pulled to low by the device. + * + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param timeout_ms The timeout to cancel waiting for the IRQ. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. + *****************************************************************************/ +static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms) +{ + ltc_t start; + Time_GetNow(&start); + + while (S2PI_ReadIrqPin(slave)) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did not " + "determine an pending interrupt within %u ms.", (uint)timeout_ms); + return ERROR_TIMEOUT; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief SPI Interrupt Test for S2PI HAL Implementation. + * + * @details This test verifies the correct implementation of the device + * integration finished interrupt callback. Therefore it configures + * the device with a minimal setup to run a pseudo measurement that + * does not emit any laser light. + * + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. + * + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible, another + * method is implemented to read if the IRQ is pending but the + * callback has not been reset yet. This is what the #S2PI_ReadIrqPin + * function is for. + * + * + * @warning The test assumes the device is in a fresh power on state and no + * additional reset is required. If the test fail, one may want to + * power cycle the device and try again. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - #ERROR_FAIL if the IRQ pin readout failed and the no or invalid + * interrupt was detected. + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. + *****************************************************************************/ +static status_t SpiInterruptTest(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 300; // timeout for measurement, might be increased.. + /*************************************************************************/ + + /* Install IRQ callback. */ + volatile bool isDataReady = false; + status_t status = S2PI_SetIrqCallback(slave, DataReadyCallback, (void *)&isDataReady); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " + "yielded error code: %d", (int)status); + return status; + } + + /* Check if IRQ is not yet pending. */ + if (S2PI_ReadIrqPin(slave) == 0) { + PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did " + "return 0 but no interrupt is pending since no " + "measurements are executed yet!"); + return ERROR_FAIL; + }; + + /* Setup Device. */ + status = ConfigureDevice(slave, 0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI interrupt test failed!"); + return status; + } + + /* Trigger Measurement. */ + status = TriggerMeasurement(slave, 0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI interrupt test failed!"); + return status; + } + + ltc_t start; + Time_GetNow(&start); + + /* Wait for Interrupt using the S2PI_ReadIrqPin method. */ + status = AwaitDataReady(slave, timeout_ms); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI interrupt test failed!"); + return status; + } + + /* Wait for Interrupt using the callback method. */ + while (!isDataReady) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + PX4_INFO_RAW("SPI interrupt test failed! The IRQ callback was not " + "invoked within %u ms.", (uint)timeout_ms); + return ERROR_TIMEOUT; + } + } + + /* Remove callback. */ + status = S2PI_SetIrqCallback(slave, 0, 0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " + "with null pointers yielded error code: %d", (int)status); + return status; + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Reads the EEPROM bytewise and applies Hamming weight. + * @details The EEPROM bytes are consecutevly read from the device via GPIO mode. + * The #EEPROM_Read function is an internal API function that enables + * the GPIO mode from the S2PI module and reads the data via a software + * bit-banging protocol. Finally it disables the GPIO mode and returns + * to SPI mode. + * + * The calls to S2PI HAL module is as follows: + * 1. S2PI_CaptureGpioControl + * 2. multiple calls to S2PI_WriteGpioPin and S2PI_ReadGpioPin + * 3. S2PI_ReleaseGpioControl + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param eeprom The 16 byte array to be filled with EEPROM data. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * interrupt was detected. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. + *****************************************************************************/ +static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) +{ + /* Enable EEPROM: */ + uint8_t d1[] = { 0x12, 0x00, 0x4B }; + status_t status = SPITransferSync(slave, d1, sizeof(d1)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " + "error code: %d", (int)status); + return status; + } + + uint8_t data[16] = { 0 }; + + /* Readout Data */ + for (uint8_t address = 0; address < 16; address++) { + status = EEPROM_Read(slave, address, &data[address]); + + if (status != STATUS_OK) { + PX4_INFO_RAW("EEPROM readout failed @ address 0x%02x, " + "error code: %d!", address, (int)status); + return status; + } + } + + /* Disable EEPROM: */ + uint8_t d2[] = { 0x12, 0x00, 0x2B }; + status = SPITransferSync(slave, d2, sizeof(d2)); + + if (status < STATUS_OK) { + PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " + "error code: %d", (int)status); + return status; + } + + /* Apply Hamming Code */ + uint8_t err = hamming_decode(data, eeprom); + + if (err != 0) { + PX4_INFO_RAW("EEPROM readout failed! Failed to decoding " + "Hamming weight (error: %d)!", err); + return STATUS_ARGUS_EEPROM_BIT_ERROR; + } + + /* Add remaining bit to the end. */ + eeprom[15] = data[15] & 0x80U; + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief GPIO Mode Test for S2PI HAL Implementation. + * + * @details This test verifies the GPIO mode of the S2PI HAL module. This is + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. + * + * This the requires several steps, most of them are already verified + * in previous tests: + * - Basic device configuration and enable EEPROM. + * - Read EERPOM via GPIO mode and apply Hamming weight + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) + * - Check if Module Number and Chip ID is not 0 + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the GPIO test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. + *****************************************************************************/ +static status_t GpioModeTest(s2pi_slave_t slave) +{ + /* Read EEPROM 3 times and verify. */ + uint8_t eeprom1[16] = { 0 }; + uint8_t eeprom2[16] = { 0 }; + uint8_t eeprom3[16] = { 0 }; + + status_t status = ReadEEPROM(slave, eeprom1); + + if (status < STATUS_OK) { + PX4_INFO_RAW("GPIO mode test failed (1st attempt)!"); + return status; + } + + status = ReadEEPROM(slave, eeprom2); + + if (status < STATUS_OK) { + PX4_INFO_RAW("GPIO mode test failed (2nd attempt)!"); + return status; + } + + status = ReadEEPROM(slave, eeprom3); + + if (status < STATUS_OK) { + PX4_INFO_RAW("GPIO mode test failed (3rd attempt)!"); + return status; + } + + /* Verify EEPROM data. */ + if ((memcmp(eeprom1, eeprom2, 16) != 0) || + (memcmp(eeprom1, eeprom3, 16) != 0)) { + PX4_INFO_RAW("GPIO Mode test failed (data comparison)!\n" + "The data from 3 distinct EEPROM readout does not match!"); + return ERROR_FAIL; + } + + /* Check EEPROM data for reasonable chip and module number (i.e. not 0) */ + uint32_t chipID = EEPROM_ReadChipId(eeprom1); + argus_module_version_t module = EEPROM_ReadModule(eeprom1); + + if (chipID == 0 || module == 0) { + PX4_INFO_RAW("GPIO Mode test failed (data verification)!\n" + "Invalid EEPROM data: Module = %d; Chip ID = %u!", module, (uint)chipID); + return ERROR_FAIL; + } + + PX4_INFO_RAW("EEPROM Readout succeeded!\n"); + PX4_INFO_RAW("- Module: %d\n", module); + PX4_INFO_RAW("- Device ID: %u\n", (uint)chipID); + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Reads the RCO_TRIM value from the devices EEPROM. + * + * @details The function reads the devices EEPROM via GPIO mode and extracts + * the RCO_TRIM value from the EEPROM map. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcotrim The read RCO_TRIM value will be returned via this pointer. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. + *****************************************************************************/ +static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) +{ + /* Read EEPROM */ + uint8_t eeprom[16] = { 0 }; + status_t status = ReadEEPROM(slave, eeprom); + + if (status != STATUS_OK) { return status; } + + argus_module_version_t module = EEPROM_ReadModule(eeprom); + + switch (module) { + case AFBR_S50MV85G_V1: + case AFBR_S50MV85G_V2: + case AFBR_S50MV85G_V3: + case AFBR_S50LV85D_V1: + case AFBR_S50MV68B_V1: + case AFBR_S50MV85I_V1: + case AFBR_S50SV85K_V1: + + /* Read RCO Trim Value from EEPROM Map 1/2/3: */ + *rcotrim = ((int8_t) eeprom[0]) >> 3; + break; + + case MODULE_NONE: /* Uncalibrated module; use all 0 data. */ + default: + + PX4_INFO_RAW("EEPROM Readout failed! Unknown module number: %d", module); + return ERROR_ARGUS_UNKNOWN_MODULE; + } + + return status; +} + +/*!*************************************************************************** + * @brief Triggers a measurement on the device and waits for the data ready + * interrupt. + * + * @details The function triggers a measurement cycle on the device and waits + * until the measurement has been finished. A \p sample count can be + * specified to setup individual number of digital averaging. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. + *****************************************************************************/ +static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples) +{ + status_t status = TriggerMeasurement(slave, samples); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Speed test failed!\n" + "Call to TransferFrame returned code: %d", + (int)status); + return status; + } + + /* Wait until the transfer is finished using a timeout. */ + status = AwaitDataReady(slave, 300); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Speed test failed!\n" + "SPI Read IRQ pin didn't raised, timeout activated at 200ms, error code: %d", + (int)status); + return status; + } + + return status; +} + +/*!*************************************************************************** + * @brief Test for Timer HAL Implementation by comparing timings to the device. + * + * @details The test verifies the timer HAL implementation by comparing the + * timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the timer test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the EEPROM Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus, + * #S2PI_SetIrqCallback, #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or #S2PI_ReadGpioPin + * return any negative status. + *****************************************************************************/ +static status_t TimerTest(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const int8_t n = 10; // The number of measurements. + const uint32_t ds = 100; // The step size in averaging samples. + const float exp_slope = 102.4; // Expected slope is 102.4 µs / phase / sample + const float rel_slope_error = 3e-2; // Relative slope tolerance is 3%. + /*************************************************************************/ + + /* Read RCOTrim value from EEPROM*/ + int8_t RcoTrim = 0; + status_t status = ReadRcoTrim(slave, &RcoTrim); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Timer test failed!\n" + "EEPROM Read test returned code: %d", (int)status); + return status; + } + + PX4_INFO_RAW("RCOTrim = %d\n", RcoTrim); + + /* Configure the device with calibrated RCO to 24MHz. */ + status = ConfigureDevice(slave, RcoTrim); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Timer test failed!\n" + "Configuration test returned code: %d", (int)status); + return status; + } + + + /* Run multiple measurements and calculate a linear regression. + * Note: this uses float types for simplicity. */ + float xsum = 0; + float ysum = 0; + float x2sum = 0; + float xysum = 0; + + PX4_INFO_RAW("+-------+---------+------------+\n"); + PX4_INFO_RAW("| count | samples | elapsed us |\n"); + PX4_INFO_RAW("+-------+---------+------------+\n"); + + for (uint8_t i = 1; i <= n; ++i) { + ltc_t start; + Time_GetNow(&start); + + int samples = ds * i; + status = RunMeasurement(slave, samples); + + if (status < STATUS_OK) { + PX4_INFO_RAW("Timer test failed!\n" + "Run measurement returned code: %d", + (int)status); + return status; + } + + uint32_t elapsed_usec = Time_GetElapsedUSec(&start); + + xsum += (float) samples; + ysum += (float) elapsed_usec; + x2sum += (float) samples * samples; + xysum += (float) samples * elapsed_usec; + + PX4_INFO_RAW("| %5d | %7d | %10d |\n", i, samples, (uint)elapsed_usec); + } + + PX4_INFO_RAW("+-------+---------+------------+\n"); + + + const float slope = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); + const float intercept = (ysum * x2sum - xsum * xysum) / (n * x2sum - xsum * xsum); + PX4_INFO_RAW("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", + (int)(10 * slope), (int)(10 * intercept)); + + /* Check the error of the slope. */ + const float max_slope = exp_slope * (1.f + rel_slope_error); + const float min_slope = exp_slope * (1.f - rel_slope_error); + + if (slope > max_slope || slope < min_slope) { + PX4_INFO_RAW("Time test failed!\n" + "The measured time slope does not match the expected value! " + "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", + (int)(10 * slope), (int)(10 * exp_slope), + (int)(10 * min_slope), (int)(10 * max_slope)); + return ERROR_FAIL; + } + + return STATUS_OK; +} + + +/*!*************************************************************************** + * @brief Data structure for the PIT test. + * + * @details Contains data that is required by the PIT timer test. + *****************************************************************************/ +typedef struct { + /*! The number of PIT callback events. */ + volatile uint32_t n; + + /*! The time stamp of the first callback event. */ + ltc_t t_first; + + /*! The time stamp of the last callback event. */ + ltc_t t_last; + +} pit_data_t; + + + +/*!*************************************************************************** + * @brief Callback function invoked by the PIT. + * + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. + * + * This implementation collects callback time stamps and counts the + * number of callback events using the abstract parameter. + * + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + *****************************************************************************/ +static void PIT_Callback(void *param) +{ + pit_data_t *data = (pit_data_t *) param; + + if (data->n == 0) { + Time_GetNow(&data->t_first); + data->t_last = data->t_first; + + } else { + Time_GetNow(&data->t_last); + } + + data->n++; +} + +/*!*************************************************************************** + * @brief Executes a PIT measurement and verifies the callback interval. + * + * @details The function configures the PIT with a given interval and waits + * several callback events to happen. In each callback event, the + * elapsed time is measured and the number of calls are counted. + * Finally, the average interrupt period is compared with the + * lifetime timer that has been already verified in a previous test + * (see #Timer_Test). + * + * @param exp_dt_us The expected timer interval in microseconds. + * @param n The number of PIT events to await. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval return any + * negative status. + *****************************************************************************/ +static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) +{ + /* Test parameter configuration: *****************************************/ + const float rel_dt_error = 1e-3; // Relative timer interval tolerance is 0.1%. + const float abs_dt_error = 1.0; // Absolute timer interval tolerance is 1us. + /*************************************************************************/ + float dt = exp_dt_us * rel_dt_error; + + if (dt < abs_dt_error) { dt = abs_dt_error; } + + const float max_dt = exp_dt_us + dt; + const float min_dt = exp_dt_us - dt; + /*************************************************************************/ + + /* Setup the PIT callback with specified interval. */ + pit_data_t data = { 0 }; + status_t status = Timer_SetInterval(exp_dt_us, &data); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", (int)status); + return status; + } + + /* Wait until n PIT callback have been happened. */ + uint32_t timeout_us = (n + 1) * exp_dt_us; + ltc_t start; + Time_GetNow(&start); + + while (data.n < n) { + if (Time_CheckTimeoutUSec(&start, timeout_us)) { + PX4_INFO_RAW("PIT test failed!\n" + "Waiting for the PIT interrupt events yielded a timeout."); + status = ERROR_TIMEOUT; + break; + } + } + + if (status == STATUS_OK) { + /* Disable the PIT timer callback. */ + status = Timer_SetInterval(0, &data); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", (int)status); + } + } + + if (status == STATUS_OK) { + /* Check if PIT callback is not invoked any more. */ + timeout_us = 2 * exp_dt_us; + Time_GetNow(&start); + + while (!Time_CheckTimeoutUSec(&start, timeout_us)) { __asm("nop"); } + + if (data.n > n) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval has been called after it was disabled."); + status = ERROR_FAIL; + } + } + + /* Verify the measured average timer interval. */ + const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (n - 1); + + if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { + PX4_INFO_RAW("PIT test failed!\n" + "The measured timer interval does not match the expected value!\n"); + status = ERROR_FAIL; + } + + PX4_INFO_RAW("PIT Test Results:\n" + " - event count: %u\n" + " - actual interval: %d us\n" + " - expected interval: %d us, min: %d us, max: %d us\n", + (uint)data.n, (int)act_dt_us, (uint)exp_dt_us, (int)min_dt, (int)max_dt); + + return status; +} + +/*!*************************************************************************** + * @brief Test for PIT HAL Implementation by comparing timings to the device. + * + * @details The test verifies the timer HAL implementation by comparing the + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval or + * #Timer_SetCallback return any negative status. + *****************************************************************************/ +static status_t PITTest(void) +{ + status_t status = Timer_SetCallback(PIT_Callback); + + if (status == ERROR_NOT_IMPLEMENTED) { return status; } + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetCallback returned status code: %d", (int)status); + return status; + } + + status = RunPITTest(10000, 10); + + if (status < STATUS_OK) { return status; } + + status = RunPITTest(333, 1000); + + if (status < STATUS_OK) { return status; } + + status = RunPITTest(100000, 5); + + if (status < STATUS_OK) { return status; } + + status = Timer_SetCallback(0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetCallback to 0 returned status code: %d", (int)status); + return status; + } + + return STATUS_OK; +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h new file mode 100644 index 000000000000..5af6660dfa58 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h @@ -0,0 +1,187 @@ +/*************************************************************************//** + * @file argus_hal_test.c + * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * + * @copyright + * + * Copyright (c) 2021, Broadcom, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder 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 HOLDER 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 ARGUS_HAL_TEST_H +#define ARGUS_HAL_TEST_H + +#ifdef __cplusplus +extern "C" { +#endif + + +/*!*************************************************************************** + * @defgroup argustest HAL Self Test + * + * @brief A test module to verify implementation of the HAL. + * + * @details A series of automated tests that can be executed on the target + * platform in order to verify the implementation of the HAL that + * are required by the API. + * + * @addtogroup argustest + * @{ + *****************************************************************************/ + +#include "argus.h" + +/*!*************************************************************************** + * @brief Version number of the HAL Self Test. + * + * @details Changes: + * * v1.0: + * - Initial release. + * * v1.1: + * - Added additional print output. + * - Increased tolerance for timer test to 3%. + * - Fixed callback issue by disabling it after IRQ test. + * * v1.1: + * - Added PIT test cases. + *****************************************************************************/ +#define HAL_TEST_VERSION "v1.2" + +/*!*************************************************************************** + * @brief Executes a series of tests in order to verify the HAL implementation. + * + * @details A series of automated tests are executed on the target platform in + * order to verify the implementation of the HAL that are required by + * the API. + * + * Each test will write an error description via the print (i.e. UART) + * function that shows what went wrong. Also an corresponding status is + * returned in case no print functionality is available. + * + * The following tests are executed: + * + * **1) Timer Plausibility Test:** + * + * Rudimentary tests of the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. + * + * **2) Timer Wraparound Test:** + * + * The LTC values must wrap from 999999 µs to 0 µs and increase the + * seconds counter accordingly. This test verifies the correct wrapping + * by consecutively calling the #Timer_GetCounterValue function until + * at least 2 wraparound events have been occurred. + * + * **3) SPI Connection Test:** + * + * This test verifies the basic functionality of the SPI interface. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. + * + * **4) SPI Interrupt Test:** + * + * This test verifies the correct implementation of the device + * integration finished interrupt callback. Therefore it configures + * the device with a minimal setup to run a pseudo measurement that + * does not emit any laser light. + * + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. + * + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible, another + * method is implemented to read if the IRQ is pending but the + * callback has not been reset yet. This is what the #S2PI_ReadIrqPin + * function is for. + * + * **5) GPIO Mode Test:** + * + * This test verifies the GPIO mode of the S2PI HAL module. This is + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. + * + * This the requires several steps, most of them are already verified + * in previous tests: + * + * - Basic device configuration and enable EEPROM. + * - Read EERPOM via GPIO mode and apply Hamming weight. + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). + * - Check if Module Number and Chip ID is not 0. + * + * **6) Timer Test for Lifetime Counter:** + * + * The test verifies the lifetime counter timer HAL implementation by + * comparing the timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. + * + * **7) Timer Test for Periodic Interrupt Timer:** + * + * The test verifies the correct implementation of the periodic + * interrupt timer (PIT). It sets different intervals and waits for + * a certain number of interrupts to happen. Each interrupt event + * is counted and the time between the first and the last interrupt + * is measured. Finally, the measured interval is compared to the + * expectations. + * + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave); + +#ifdef __cplusplus +} +#endif + +/*! @} */ +#endif /* ARGUS_CAL_API_H */ diff --git a/src/drivers/distance_sensor/cm8jl65/Kconfig b/src/drivers/distance_sensor/cm8jl65/Kconfig new file mode 100644 index 000000000000..2da9e8f79c0f --- /dev/null +++ b/src/drivers/distance_sensor/cm8jl65/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_CM8JL65 + bool "cm8jl65" + default n + ---help--- + Enable support for cm8jl65 \ No newline at end of file diff --git a/src/drivers/distance_sensor/gy_us42/CMakeLists.txt b/src/drivers/distance_sensor/gy_us42/CMakeLists.txt new file mode 100644 index 000000000000..7d59f37cb485 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__distance_sensor__gy_us42 + MAIN gy_us42 + SRCS + GY_US42.cpp + GY_US42.hpp + GY_US42_main.cpp + DEPENDS + drivers_rangefinder + px4_work_queue + ) diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42.cpp b/src/drivers/distance_sensor/gy_us42/GY_US42.cpp new file mode 100644 index 000000000000..4c3198cd425e --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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 "GY_US42.hpp" + +GY_US42::GY_US42(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) +{ + _px4_rangefinder.set_max_distance(GY_US42_MAX_DISTANCE); + _px4_rangefinder.set_min_distance(GY_US42_MIN_DISTANCE); +} + +GY_US42::~GY_US42() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int GY_US42::init() +{ + // I2C init (and probe) first. + if (I2C::init() != OK) { + return PX4_ERROR; + } + + _state = STATE::POWERON_WAIT; + ScheduleDelayed(2000); + return OK; +} + +int GY_US42::collect() +{ + // Read from the sensor. + uint8_t val[2] = {}; + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = transfer(nullptr, 0, val, 2); + + if (ret < 0) { + PX4_DEBUG("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; + + _px4_rangefinder.update(timestamp_sample, distance_m); + + perf_end(_sample_perf); + return PX4_OK; +} + +int GY_US42::measure() +{ + uint8_t cmd[1] = {GY_US42_TAKE_RANGE_REG}; + + // Send the command to begin a measurement. + int ret = transfer(cmd, 1, nullptr, 0); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + return ret; + } + + return PX4_OK; +} + +void GY_US42::RunImpl() +{ + switch (_state) { + case STATE::INIT: + // do nothing + break; + + case STATE::POWERON_WAIT: + measure(); + _state = STATE::MEASURE_WAIT; + ScheduleOnInterval(GY_US42_CONVERSION_INTERVAL, GY_US42_CONVERSION_INTERVAL); + break; + + case STATE::MEASURE_WAIT: + collect(); + measure(); + // forever loop + break; + + case STATE::MODIFYADDR_WAIT: + break; + } +} + +void GY_US42::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42.hpp b/src/drivers/distance_sensor/gy_us42/GY_US42.hpp new file mode 100644 index 000000000000..4aeb007db6cb --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * 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 GY_US42.cpp + * + * Driver for the GY-US42 sonar range finder on I2C. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ +#define GY_US42_BASEADDR 0x70 // 7-bit address. 8-bit address is 0xE0. + +/* GY_US42 Registers addresses */ +#define GY_US42_TAKE_RANGE_REG 0x51 // Measure range Register. +#define GY_US42_SET_ADDRESS_CMD1 0xAA // Change address 1 cmd. +#define GY_US42_SET_ADDRESS_CMD2 0xA5 // Change address 2 cmd. + +/* Device limits */ +#define GY_US42_MIN_DISTANCE (0.20f) +#define GY_US42_MAX_DISTANCE (7.2f) + +#define GY_US42_CONVERSION_INTERVAL 50000 // 50ms for one sonar. + +class GY_US42 : public device::I2C, public I2CSPIDriver +{ +public: + GY_US42(const I2CSPIDriverConfig &config); + ~GY_US42() override; + + static void print_usage(); + + int init() override; + void print_status() override; + + void RunImpl(); + +private: + + enum class STATE : uint8_t { + INIT, + POWERON_WAIT, + MEASURE_WAIT, + MODIFYADDR_WAIT + }; + STATE _state{STATE::INIT}; + + int collect(); + int measure(); + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + + PX4Rangefinder _px4_rangefinder; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; +}; diff --git a/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp b/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp new file mode 100644 index 000000000000..8e660d90c761 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/GY_US42_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 "GY_US42.hpp" + +#include +#include + +void +GY_US42::print_usage() +{ + PRINT_MODULE_USAGE_NAME("gy_us42", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" __EXPORT int gy_us42_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = GY_US42; + BusCLIArguments cli{true, false}; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.default_i2c_frequency = 100000; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_GY_US42); + + 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/distance_sensor/gy_us42/Kconfig b/src/drivers/distance_sensor/gy_us42/Kconfig new file mode 100644 index 000000000000..84ffb727e6f8 --- /dev/null +++ b/src/drivers/distance_sensor/gy_us42/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_GY_US42 + bool "GY_US42" + default n + ---help--- + Enable support for GY_US42 diff --git a/src/drivers/distance_sensor/leddar_one/Kconfig b/src/drivers/distance_sensor/leddar_one/Kconfig new file mode 100644 index 000000000000..8b8cd271ccfc --- /dev/null +++ b/src/drivers/distance_sensor/leddar_one/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LEDDAR_ONE + bool "leddar_one" + default n + ---help--- + Enable support for leddar_one \ No newline at end of file diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/Kconfig b/src/drivers/distance_sensor/lightware_laser_i2c/Kconfig new file mode 100644 index 000000000000..ecbf154b0a0c --- /dev/null +++ b/src/drivers/distance_sensor/lightware_laser_i2c/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C + bool "lightware_laser_i2c" + default n + ---help--- + Enable support for lightware_laser_i2c \ No newline at end of file diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 57b720ec6d6a..2e4c6ba97d9c 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2016, 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 @@ -60,13 +60,10 @@ using namespace time_literals; class LightwareLaser : public device::I2C, public I2CSPIDriver { public: - LightwareLaser(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, - int address = LIGHTWARE_LASER_BASEADDR); + LightwareLaser(const I2CSPIDriverConfig &config); ~LightwareLaser() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; @@ -132,11 +129,10 @@ class LightwareLaser : public device::I2C, public I2CSPIDriver int _consecutive_errors{0}; }; -LightwareLaser::LightwareLaser(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, - int address) : - I2C(DRV_DIST_DEVTYPE_LIGHTWARE_LASER, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), rotation) +LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER); } @@ -200,12 +196,18 @@ int LightwareLaser::init() break; default: - PX4_ERR("invalid HW model %d.", hw_model); + PX4_ERR("invalid HW model %" PRId32 ".", hw_model); return ret; } /* do I2C init (and probe) first */ - return I2C::init(); + ret = I2C::init(); + + if (ret == PX4_OK) { + start(); + } + + return ret; } int LightwareLaser::readRegister(Register reg, uint8_t *data, int len) @@ -265,7 +267,7 @@ int LightwareLaser::enableI2CBinaryProtocol() return ret; } - PX4_DEBUG("protocol values: 0x%x 0x%x", value[0], value[1]); + PX4_DEBUG("protocol values: 0x%" PRIx8 " 0x%" PRIx8, value[0], value[1]); return (value[0] == 0xcc && value[1] == 0x00) ? 0 : -1; } @@ -348,7 +350,7 @@ int LightwareLaser::collect() perf_end(_sample_perf); // compare different outputs (median filter adds about 25ms delay) - PX4_DEBUG("fm: %4i, fs: %2i%%, lm: %4i, lr: %4i, fs: %2i%%, n: %i", + PX4_DEBUG("fm: %4" PRIi16 ", fs: %2" PRIi16 "%%, lm: %4" PRIi16 ", lr: %4" PRIi16 ", fs: %2" PRIi16 "%%, n: %" PRIi16, data.first_return_median, data.first_return_strength, data.last_return_median, data.last_return_raw, data.last_return_strength, data.background_noise); @@ -422,46 +424,29 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x66); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LightwareLaser::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LightwareLaser* instance = new LightwareLaser(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int lightware_laser_i2c_main(int argc, char *argv[]) { int ch; using ThisDriver = LightwareLaser; BusCLIArguments cli{true, false}; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.default_i2c_frequency = 400000; + cli.i2c_address = LIGHTWARE_LASER_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = atoi(cli.optarg()); + cli.rotation = (Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/lightware_laser_serial/Kconfig b/src/drivers/distance_sensor/lightware_laser_serial/Kconfig new file mode 100644 index 000000000000..aca223c8284d --- /dev/null +++ b/src/drivers/distance_sensor/lightware_laser_serial/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL + bool "lightware_laser_serial" + default n + ---help--- + Enable support for lightware_laser_serial \ No newline at end of file diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp index 34716cbc2e0f..359a62f575e0 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp +++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019, 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,6 +33,7 @@ #include "lightware_laser_serial.hpp" +#include #include #include @@ -126,8 +127,15 @@ LightwareLaserSerial::init() _simple_serial = true; break; + case 8: + /* LW20/c (100M 20Hz) */ + _px4_rangefinder.set_min_distance(0.2f); + _px4_rangefinder.set_max_distance(100.0f); + _interval = 50000; + break; + default: - PX4_ERR("invalid HW model %d.", hw_model); + PX4_ERR("invalid HW model %" PRIi32 ".", hw_model); return -1; } @@ -296,6 +304,12 @@ void LightwareLaserSerial::Run() if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { PX4_ERR("baud %d ATTR", termios_state); } + + // LW20: Enable serial mode by sending some characters + if (hw_model == 8) { + const char *data = "www\r\n"; + (void)!::write(_fd, &data, strlen(data)); + } } /* collection phase? */ diff --git a/src/drivers/distance_sensor/lightware_laser_serial/parameters.c b/src/drivers/distance_sensor/lightware_laser_serial/parameters.c index a59d80600c44..f8b2c01a8ed4 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/parameters.c +++ b/src/drivers/distance_sensor/lightware_laser_serial/parameters.c @@ -41,5 +41,8 @@ * @value 3 SF10/b * @value 4 SF10/c * @value 5 SF11/c + * @value 6 SF30/b + * @value 7 SF30/c + * @value 8 LW20/c */ PARAM_DEFINE_INT32(SENS_EN_SF0X, 1); diff --git a/src/drivers/distance_sensor/ll40ls/Kconfig b/src/drivers/distance_sensor/ll40ls/Kconfig new file mode 100644 index 000000000000..297a86147a66 --- /dev/null +++ b/src/drivers/distance_sensor/ll40ls/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LL40LS + bool "ll40ls" + default n + ---help--- + Enable support for ll40ls \ No newline at end of file diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 562afbe36d73..2c037657387a 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019, 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 @@ -41,17 +41,14 @@ #include "LidarLiteI2C.h" -LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency, - const int address) : - I2C(DRV_RNG_DEVTYPE_LL40LS, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), orientation) +LidarLiteI2C::LidarLiteI2C(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian - // up the retries since the device misses the first measure attempts - _retries = 3; _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LL40LS); /// TODO } @@ -72,6 +69,7 @@ LidarLiteI2C::init() return PX4_ERROR; } + start(); return PX4_OK; } @@ -83,7 +81,7 @@ LidarLiteI2C::print_status() perf_print_counter(_comms_errors); perf_print_counter(_sensor_resets); perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u \n", get_measure_interval()); + printf("poll interval: %" PRIu32 "\n", get_measure_interval()); } int @@ -126,9 +124,6 @@ LidarLiteI2C::probe() uint8_t id_high = 0; uint8_t id_low = 0; - // more retries for detection - _retries = 10; - for (uint8_t i = 0; i < sizeof(addresses); i++) { set_device_address(addresses[i]); @@ -140,8 +135,35 @@ LidarLiteI2C::probe() * v1 and v3 don't have the unit id register while v2 has both. * It would be better if we had a proper WHOAMI register. */ + + if (read_reg(LL40LS_HW_VERSION_V4, _hw_version) == OK && + read_reg(LL40LS_SW_VERSION_V4, _sw_version) == OK) { + if (_hw_version > 0) { + PX4_DEBUG("probe success - hw: %u, sw: %u", _hw_version, _sw_version); + + uint8_t id_byte_0 = 0; + uint8_t id_byte_1 = 0; + uint8_t id_byte_2 = 0; + uint8_t id_byte_3 = 0; + + if (read_reg(LL40LS_UNIT_ID_0_V4, id_byte_0) == OK && + read_reg(LL40LS_UNIT_ID_1_V4, id_byte_1) == OK && + read_reg(LL40LS_UNIT_ID_2_V4, id_byte_2) == OK && + read_reg(LL40LS_UNIT_ID_3_V4, id_byte_3) == OK) { + _unit_id = ((uint32_t)id_byte_3 << 24) | ((uint32_t)id_byte_2 << 16) | ((uint32_t)id_byte_1 << 8) | + (uint32_t)id_byte_0; + } + + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V4); + _model = Model::v4; + return OK; + } + + } + if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) && (read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) { + PX4_DEBUG("hw: %u - sw %u \n", _hw_version, _sw_version); if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK && read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) { @@ -152,31 +174,29 @@ LidarLiteI2C::probe() if (_unit_id > 0) { // v2 - PX4_INFO("probe success - hw: %u, sw:%u, id: %u", _hw_version, _sw_version, _unit_id); + PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8 ", id: %" PRIu16, _hw_version, _sw_version, _unit_id); _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2); } else { // v1 and v3 - PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version); + PX4_DEBUG("probe success - hw: %" PRIu8 ", sw:%" PRIu8, _hw_version, _sw_version); } } else { if (_unit_id > 0) { // v3hp - _is_v3hp = true; - PX4_INFO("probe success - id: %u", _unit_id); + _model = Model::v3hp; + PX4_DEBUG("probe success - id: %" PRIu16, _unit_id); } } - _retries = 3; + _retries = 1; return OK; } - PX4_DEBUG("probe failed unit_id=0x%02x hw_version=0x%02x sw_version=0x%02x", - (unsigned)_unit_id, - (unsigned)_hw_version, - (unsigned)_sw_version); + PX4_DEBUG("probe failed unit_id=0x%02" PRIx16 " hw_version=0x%02" PRIu8 " sw_version=0x%02" PRIu8, + _unit_id, _hw_version, _sw_version); } @@ -273,10 +293,10 @@ LidarLiteI2C::print_registers() int ret = lidar_transfer(®, 1, &val, 1); if (ret != OK) { - printf("%02x:XX ", (unsigned)reg); + printf("%02" PRIx8 ":XX ", reg); } else { - printf("%02x:%02x ", (unsigned)reg, (unsigned)val); + printf("%02" PRIx8 ":%02" PRIu8, reg, val); } if (reg % 16 == 15) { @@ -291,18 +311,29 @@ LidarLiteI2C::print_registers() int LidarLiteI2C::collect() { + // read from the sensor uint8_t val[2] {}; + int ret = 0; perf_begin(_sample_perf); + uint8_t invalid_distance_index = 0; + // read the high and low byte distance registers - uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT; - int ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); + if (_model == Model::v4) { + uint8_t distance_reg = LL40LS_DISTHIGH_REG_V4; + ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); + invalid_distance_index = 1; + + } else { + uint8_t distance_reg = LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT; + ret = lidar_transfer(&distance_reg, 1, &val[0], sizeof(val)); + } // if the transfer failed or if the high bit of distance is // set then the distance is invalid - if (ret < 0 || (val[0] & 0x80)) { + if (ret < 0 || (val[invalid_distance_index] & 0x80)) { if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT) { /* NACKs from the sensor are expected when we @@ -324,7 +355,15 @@ LidarLiteI2C::collect() return ret; } - uint16_t distance_cm = (val[0] << 8) | val[1]; + uint16_t distance_cm = 0; + + if (_model == Model::v4) { + distance_cm = ((uint16_t)(val[1] << 8)) | (uint16_t)val[0]; + + } else { + distance_cm = (val[0] << 8) | val[1]; + } + const float distance_m = float(distance_cm) * 1e-2f; if (distance_cm == 0) { @@ -351,61 +390,25 @@ LidarLiteI2C::collect() // this should be fairly close to the end of the measurement, so the best approximation of the time const hrt_abstime timestamp_sample = hrt_absolute_time(); - // Relative signal strength measurement, i.e. the strength of - // the main signal peak compared to the general noise level. - uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG; - ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1); + int8_t signal_quality = -1; - // check if the transfer failed - if (ret < 0) { - if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) { - /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - PX4_INFO("signal strength read failed"); - - DEVICE_DEBUG("error reading signal strength from sensor: %d", ret); - perf_count(_comms_errors); - - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } - } - - perf_end(_sample_perf); - // if we are getting lots of I2C transfer errors try - // resetting the sensor - return ret; - } - - uint8_t ll40ls_signal_strength = val[0]; - uint8_t signal_quality; - - // We detect if V3HP is being used - if (_is_v3hp) { - //Normalize signal strength to 0...100 percent using the absolute signal strength. - signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) / - (LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP); - - } else { - // Absolute peak strength measurement, i.e. absolute strength of main signal peak. - uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG; - ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1); + if (_model != Model::v4) { + // Relative signal strength measurement, i.e. the strength of + // the main signal peak compared to the general noise level. + uint8_t signal_strength_reg = LL40LS_SIGNAL_STRENGTH_REG; + ret = lidar_transfer(&signal_strength_reg, 1, &val[0], 1); // check if the transfer failed if (ret < 0) { if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) { /* - NACKs from the sensor are expected when we - read before it is ready, so only consider it - an error if more than 100ms has elapsed. - */ - PX4_INFO("peak strength read failed"); + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + PX4_INFO("signal strength read failed"); - DEVICE_DEBUG("error reading peak strength from sensor: %d", ret); + DEVICE_DEBUG("error reading signal strength from sensor: %d", ret); perf_count(_comms_errors); if (perf_event_count(_comms_errors) % 10 == 0) { @@ -420,18 +423,58 @@ LidarLiteI2C::collect() return ret; } - uint8_t ll40ls_peak_strength = val[0]; + uint8_t ll40ls_signal_strength = val[0]; - // For v2 and v3 use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements - if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) { - signal_quality = 0; + // We detect if V3HP is being used + if (_model == Model::v3hp) { + //Normalize signal strength to 0...100 percent using the absolute signal strength. + signal_quality = 100 * math::max(ll40ls_signal_strength - LL40LS_SIGNAL_STRENGTH_MIN_V3HP, 0) / + (LL40LS_SIGNAL_STRENGTH_MAX_V3HP - LL40LS_SIGNAL_STRENGTH_MIN_V3HP); } else { - //Normalize signal strength to 0...100 percent using the absolute signal peak strength. - signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) / - (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW); + // Absolute peak strength measurement, i.e. absolute strength of main signal peak. + uint8_t peak_strength_reg = LL40LS_PEAK_STRENGTH_REG; + ret = lidar_transfer(&peak_strength_reg, 1, &val[0], 1); + + // check if the transfer failed + if (ret < 0) { + if (hrt_elapsed_time(&_acquire_time_usec) > LL40LS_CONVERSION_TIMEOUT) { + /* + NACKs from the sensor are expected when we + read before it is ready, so only consider it + an error if more than 100ms has elapsed. + */ + PX4_INFO("peak strength read failed"); + + DEVICE_DEBUG("error reading peak strength from sensor: %d", ret); + perf_count(_comms_errors); + + if (perf_event_count(_comms_errors) % 10 == 0) { + perf_count(_sensor_resets); + reset_sensor(); + } + } + + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } + uint8_t ll40ls_peak_strength = val[0]; + + // For v2 and v3 use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements + if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW || distance_m < LL40LS_MIN_DISTANCE) { + signal_quality = 0; + + } else { + //Normalize signal strength to 0...100 percent using the absolute signal peak strength. + signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, 0) / + (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW); + + } } + } _px4_rangefinder.update(timestamp_sample, distance_m, signal_quality); diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index b43cea4deacf..0a4fd473039c 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -83,6 +83,15 @@ static constexpr int LL40LS_PEAK_STRENGTH_HIGH = 234; /* Max peak strengt static constexpr float LL40LS_MIN_DISTANCE{0.05f}; static constexpr float LL40LS_MAX_DISTANCE{25.00f}; static constexpr float LL40LS_MAX_DISTANCE_V2{35.00f}; +static constexpr float LL40LS_MAX_DISTANCE_V4{10.00f}; + +static constexpr uint8_t LL40LS_HW_VERSION_V4 = 0xE1; +static constexpr uint8_t LL40LS_SW_VERSION_V4 = 0x30; +static constexpr uint8_t LL40LS_UNIT_ID_0_V4 = 0x16; +static constexpr uint8_t LL40LS_UNIT_ID_1_V4 = 0x17; +static constexpr uint8_t LL40LS_UNIT_ID_2_V4 = 0x18; +static constexpr uint8_t LL40LS_UNIT_ID_3_V4 = 0x19; +static constexpr uint8_t LL40LS_DISTHIGH_REG_V4 = 0x10; /* High byte of distance register, auto increment */ // Normal conversion wait time. static constexpr uint32_t LL40LS_CONVERSION_INTERVAL{50_ms}; @@ -94,12 +103,9 @@ static constexpr uint32_t LL40LS_CONVERSION_TIMEOUT{100_ms}; class LidarLiteI2C : public device::I2C, public I2CSPIDriver { public: - LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint8_t orientation, int bus_frequency, - const int address = LL40LS_BASEADDR); + LidarLiteI2C(const I2CSPIDriverConfig &config); virtual ~LidarLiteI2C(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); @@ -171,13 +177,20 @@ class LidarLiteI2C : public device::I2C, public I2CSPIDriver int probe_address(const uint8_t address); bool _collect_phase{false}; - bool _is_v3hp{false}; bool _pause_measurements{false}; + enum class Model { + Generic = 0, + v3hp = 1, + v4 = 2, + }; + + Model _model{Model::Generic}; + uint8_t _hw_version{0}; uint8_t _sw_version{0}; - uint16_t _unit_id{0}; + uint32_t _unit_id{0}; uint16_t _zero_counter{0}; uint64_t _acquire_time_usec{0}; diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 2cc80fa0fee5..0a4dc940015c 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -64,30 +64,12 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/lidar_lite.html PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x62); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_COMMAND("regdump"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LidarLiteI2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LidarLiteI2C* instance = new LidarLiteI2C(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - void LidarLiteI2C::custom_method(const BusCLIArguments &cli) { @@ -99,18 +81,19 @@ extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]) int ch; using ThisDriver = LidarLiteI2C; BusCLIArguments cli{true, false}; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.default_i2c_frequency = 100000; + cli.i2c_address = LL40LS_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/ll40ls_pwm/Kconfig b/src/drivers/distance_sensor/ll40ls_pwm/Kconfig new file mode 100644 index 000000000000..543d57f80383 --- /dev/null +++ b/src/drivers/distance_sensor/ll40ls_pwm/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_LL40LS_PWM + bool "ll40ls_pwm" + default n + ---help--- + Enable support for ll40ls_pwm \ No newline at end of file diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 0d3a4a075410..4006e6640125 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019, 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 @@ -145,7 +145,7 @@ LidarLitePWM::print_info() perf_print_counter(_comms_errors); perf_print_counter(_sensor_resets); perf_print_counter(_sensor_zero_resets); - printf("poll interval: %u \n", get_measure_interval()); + printf("poll interval: %" PRIu32 " \n", get_measure_interval()); } #endif diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h index 1da621f8e957..55786ab4cfe0 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2019, 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 diff --git a/src/drivers/distance_sensor/mappydot/Kconfig b/src/drivers/distance_sensor/mappydot/Kconfig new file mode 100644 index 000000000000..8e13c2fc44b4 --- /dev/null +++ b/src/drivers/distance_sensor/mappydot/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_MAPPYDOT + bool "mappydot" + default n + ---help--- + Enable support for mappydot \ No newline at end of file diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 86a647743abb..3144237ab899 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -144,11 +144,9 @@ using namespace time_literals; class MappyDot : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - MappyDot(I2CSPIBusOption bus_option, const int bus, int bus_frequency); + MappyDot(const I2CSPIDriverConfig &config); virtual ~MappyDot(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); /** @@ -221,10 +219,10 @@ class MappyDot : public device::I2C, public ModuleParams, public I2CSPIDriverinit() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int mappydot_main(int argc, char *argv[]) { using ThisDriver = MappyDot; @@ -466,6 +443,8 @@ extern "C" __EXPORT int mappydot_main(int argc, char *argv[]) return -1; } + cli.i2c_address = MAPPYDOT_BASE_ADDR; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_MAPPYDOT); if (!strcmp(verb, "start")) { diff --git a/src/drivers/distance_sensor/mb12xx/Kconfig b/src/drivers/distance_sensor/mb12xx/Kconfig new file mode 100644 index 000000000000..01eb0c2bfe16 --- /dev/null +++ b/src/drivers/distance_sensor/mb12xx/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_MB12XX + bool "mb12xx" + default n + ---help--- + Enable support for mb12xx \ No newline at end of file diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index a94b954d59d5..da8d5321b0d5 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -88,14 +88,12 @@ using namespace time_literals; class MB12XX : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - MB12XX(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address); + MB12XX(const I2CSPIDriverConfig &config); virtual ~MB12XX(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); - virtual int init() override; + int init() override; /** * Diagnostics - print some basic information about the driver. @@ -175,10 +173,10 @@ class MB12XX : public device::I2C, public ModuleParams, public I2CSPIDriver 1) { _measure_interval = MB12XX_INTERVAL_BETWEEN_SUCCESIVE_FIRES; } PX4_INFO("Total sensors connected: %i", _sensor_count); + start(); return PX4_OK; } @@ -407,26 +406,6 @@ MB12XX::custom_method(const BusCLIArguments &cli) set_address(cli.i2c_address); } -I2CSPIDriverBase *MB12XX::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MB12XX *instance = new MB12XX(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - - void MB12XX::print_usage() { @@ -434,7 +413,6 @@ MB12XX::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x70); PRINT_MODULE_USAGE_COMMAND("set_address"); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x70); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -444,7 +422,6 @@ extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]) { using ThisDriver = MB12XX; BusCLIArguments cli{true, false}; - cli.i2c_address = MB12XX_BASE_ADDR; cli.default_i2c_frequency = MB12XX_BUS_SPEED; const char *verb = cli.parseDefaultArguments(argc, argv); @@ -454,6 +431,8 @@ extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]) return -1; } + cli.i2c_address = MB12XX_BASE_ADDR; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIST_DEVTYPE_MB12XX); if (!strcmp(verb, "start")) { diff --git a/src/drivers/distance_sensor/pga460/Kconfig b/src/drivers/distance_sensor/pga460/Kconfig new file mode 100644 index 000000000000..de9f2ef5392e --- /dev/null +++ b/src/drivers/distance_sensor/pga460/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_PGA460 + bool "pga460" + default n + ---help--- + Enable support for pga460 \ No newline at end of file diff --git a/src/drivers/distance_sensor/srf02/Kconfig b/src/drivers/distance_sensor/srf02/Kconfig new file mode 100644 index 000000000000..258dec353ff9 --- /dev/null +++ b/src/drivers/distance_sensor/srf02/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_SRF02 + bool "srf02" + default n + ---help--- + Enable support for srf02 \ No newline at end of file diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index 6b223072c6d6..70ef61833b48 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/src/drivers/distance_sensor/srf02/SRF02.cpp @@ -33,10 +33,10 @@ #include "SRF02.hpp" -SRF02::SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : - I2C(DRV_DIST_DEVTYPE_SRF02, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), rotation) +SRF02::SRF02(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF02); _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND); @@ -69,7 +69,13 @@ int SRF02::init() // XXX we should find out why we need to wait 200 ms here px4_usleep(200000); - return measure(); + int ret = measure(); + + if (ret == PX4_OK) { + start(); + } + + return ret; } int SRF02::collect() diff --git a/src/drivers/distance_sensor/srf02/SRF02.hpp b/src/drivers/distance_sensor/srf02/SRF02.hpp index e074a02f8197..798ddaaaaf35 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.hpp +++ b/src/drivers/distance_sensor/srf02/SRF02.hpp @@ -65,12 +65,9 @@ class SRF02 : public device::I2C, public I2CSPIDriver { public: - SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, - int address = SRF02_BASEADDR); + SRF02(const I2CSPIDriverConfig &config); ~SRF02() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; diff --git a/src/drivers/distance_sensor/srf02/srf02_main.cpp b/src/drivers/distance_sensor/srf02/srf02_main.cpp index 30d558a55d23..6c49020be75a 100644 --- a/src/drivers/distance_sensor/srf02/srf02_main.cpp +++ b/src/drivers/distance_sensor/srf02/srf02_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020, 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 @@ -43,46 +43,29 @@ SRF02::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x70); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *SRF02::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - SRF02 *instance = new SRF02(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int srf02_main(int argc, char *argv[]) { int ch; using ThisDriver = SRF02; BusCLIArguments cli{true, false}; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.default_i2c_frequency = 100000; + cli.i2c_address = SRF02_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = atoi(cli.optarg()); + cli.rotation = (Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/srf05/Kconfig b/src/drivers/distance_sensor/srf05/Kconfig new file mode 100644 index 000000000000..a425ccb9fc0a --- /dev/null +++ b/src/drivers/distance_sensor/srf05/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_SRF05 + bool "srf05" + default n + ---help--- + Enable support for srf05 \ No newline at end of file diff --git a/src/drivers/distance_sensor/srf05/SRF05.cpp b/src/drivers/distance_sensor/srf05/SRF05.cpp index 76ba2eb50280..9612601057d2 100644 --- a/src/drivers/distance_sensor/srf05/SRF05.cpp +++ b/src/drivers/distance_sensor/srf05/SRF05.cpp @@ -152,7 +152,7 @@ SRF05::measure() const hrt_abstime timestamp_sample = hrt_absolute_time(); const hrt_abstime dt = _falling_edge_time - _rising_edge_time; - const float current_distance = dt * 343.0f / 10e6f / 2.0f; + const float current_distance = dt * 343.0f / 1e6f / 2.0f; if (dt > HXSRX0X_CONVERSION_TIMEOUT) { perf_count(_comms_errors); @@ -245,7 +245,7 @@ SRF05::print_status() perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_sensor_resets); - printf("poll interval: %u \n", get_measure_interval()); + printf("poll interval: %" PRIu32 " \n", get_measure_interval()); return 0; } diff --git a/src/drivers/distance_sensor/teraranger/Kconfig b/src/drivers/distance_sensor/teraranger/Kconfig new file mode 100644 index 000000000000..cee7073c3d6e --- /dev/null +++ b/src/drivers/distance_sensor/teraranger/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_TERARANGER + bool "teraranger" + default n + ---help--- + Enable support for teraranger \ No newline at end of file diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index 507a8460f4a8..8c5e9b3d0c8a 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -73,14 +73,11 @@ static uint8_t crc8(uint8_t *p, uint8_t len) return crc & 0xFF; } -TERARANGER::TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency) : - I2C(DRV_DIST_DEVTYPE_TERARANGER, MODULE_NAME, bus, TERARANGER_ONE_BASEADDR, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), rotation) +TERARANGER::TERARANGER(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { - // up the retries since the device misses the first measure attempts - I2C::_retries = 3; - _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TERARANGER); _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); } @@ -208,10 +205,12 @@ int TERARANGER::init() break; default: - PX4_ERR("invalid HW model %d.", hw_model); + PX4_ERR("invalid HW model %" PRId32 ".", hw_model); return PX4_ERROR; } + start(); + return PX4_OK; } @@ -240,13 +239,13 @@ int TERARANGER::probe() // Can't use a single transfer as Teraranger needs a bit of time for internal processing. if (transfer(&cmd, 1, nullptr, 0) == OK) { if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TERARANGER_WHO_AM_I_REG_VAL) { + _retries = 1; return measure(); } } - PX4_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", - (unsigned)who_am_i, - TERARANGER_WHO_AM_I_REG_VAL); + PX4_DEBUG("WHO_AM_I byte mismatch 0x%02" PRIx8 " should be 0x%02" PRIx8 "\n", + who_am_i, TERARANGER_WHO_AM_I_REG_VAL); // Not found on any address. return -EIO; diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.hpp b/src/drivers/distance_sensor/teraranger/TERARANGER.hpp index 6351f30cd4fb..67dad8c11595 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.hpp +++ b/src/drivers/distance_sensor/teraranger/TERARANGER.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -75,11 +75,9 @@ using namespace time_literals; class TERARANGER : public device::I2C, public I2CSPIDriver { public: - TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency); + TERARANGER(const I2CSPIDriverConfig &config); ~TERARANGER() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); virtual int init() override; diff --git a/src/drivers/distance_sensor/teraranger/teraranger_main.cpp b/src/drivers/distance_sensor/teraranger/teraranger_main.cpp index 55582a27ee48..0b0efcaa7ec3 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger_main.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020, 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 @@ -53,46 +53,29 @@ Setup/usage information: https://docs.px4.io/master/en/sensor/rangefinders.html# PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *TERARANGER::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - TERARANGER *instance = new TERARANGER(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int teraranger_main(int argc, char *argv[]) { int ch; using ThisDriver = TERARANGER; BusCLIArguments cli{true, false}; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.default_i2c_frequency = 100000; + cli.i2c_address = TERARANGER_ONE_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = atoi(cli.optarg()); + cli.rotation = (Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/tfmini/Kconfig b/src/drivers/distance_sensor/tfmini/Kconfig new file mode 100644 index 000000000000..9157e4a216c9 --- /dev/null +++ b/src/drivers/distance_sensor/tfmini/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_TFMINI + bool "tfmini" + default n + ---help--- + Enable support for tfmini \ No newline at end of file diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index d20a20b4c2db..150d1c486ddd 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019, 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 @@ -88,7 +88,7 @@ TFMINI::init() break; default: - PX4_ERR("invalid HW model %d.", hw_model); + PX4_ERR("invalid HW model %" PRId32 ".", hw_model); return -1; } diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.hpp b/src/drivers/distance_sensor/tfmini/TFMINI.hpp index ba1e0055c2c6..027d73fa5248 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.hpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019, 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 diff --git a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp index ab65a0c583e0..76f754634a76 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp @@ -159,7 +159,11 @@ int tfmini_parse(char c, char *parserbuf, unsigned *parserbuf_index, TFMINI_PARS unsigned int t2 = parserbuf[3]; t2 <<= 8; t2 += t1; - *dist = ((float)t2) / 100; + + if (t2 < 0xFFFFu) { + *dist = ((float)t2) / 100; + } + *state = TFMINI_PARSE_STATE::STATE6_GOT_CHECKSUM; *parserbuf_index = 0; ret = 0; diff --git a/src/drivers/distance_sensor/ulanding_radar/Kconfig b/src/drivers/distance_sensor/ulanding_radar/Kconfig new file mode 100644 index 000000000000..f2b51c843d98 --- /dev/null +++ b/src/drivers/distance_sensor/ulanding_radar/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_ULANDING_RADAR + bool "ulanding_radar" + default n + ---help--- + Enable support for ulanding_radar \ No newline at end of file diff --git a/src/drivers/distance_sensor/ulanding_radar/module.yaml b/src/drivers/distance_sensor/ulanding_radar/module.yaml index 2092bbb2a1f3..73dc25174921 100644 --- a/src/drivers/distance_sensor/ulanding_radar/module.yaml +++ b/src/drivers/distance_sensor/ulanding_radar/module.yaml @@ -1,6 +1,6 @@ module_name: uLanding Radar serial_config: - - command: ulanding_radar start ${SERIAL_DEV} + - command: ulanding_radar start -d ${SERIAL_DEV} port_config_param: name: SENS_ULAND_CFG group: Sensors diff --git a/src/drivers/distance_sensor/vl53l0x/Kconfig b/src/drivers/distance_sensor/vl53l0x/Kconfig new file mode 100644 index 000000000000..98dac82b0069 --- /dev/null +++ b/src/drivers/distance_sensor/vl53l0x/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_VL53L0X + bool "vl53l0x" + default n + ---help--- + Enable support for vl53l0x \ No newline at end of file diff --git a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp index f15b2dc62959..6a6c5a3580ae 100644 --- a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp +++ b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp @@ -59,18 +59,18 @@ #define VL53L0X_BUS_CLOCK 400000 // 400kHz bus clock speed -VL53L0X::VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : - I2C(DRV_DIST_DEVTYPE_VL53L0X, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), rotation) +VL53L0X::VL53L0X(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { // VL53L0X typical range 0-2 meters with 25 degree field of view _px4_rangefinder.set_min_distance(0.f); _px4_rangefinder.set_max_distance(2.f); _px4_rangefinder.set_fov(math::radians(25.f)); - // Allow 3 retries as the device typically misses the first measure attempts. - I2C::_retries = 3; + // Allow retries as the device typically misses the first measure attempts. + I2C::_retries = 1; _px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_VL53L0X); } @@ -82,6 +82,18 @@ VL53L0X::~VL53L0X() perf_free(_comms_errors); } +int VL53L0X::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + return ret; + } + + ScheduleNow(); + return PX4_OK; +} + int VL53L0X::collect() { // Read from the sensor. @@ -489,12 +501,6 @@ int VL53L0X::singleRefCalibration(const uint8_t byte) return PX4_OK; } -void VL53L0X::start() -{ - // Schedule the first cycle. - ScheduleNow(); -} - int VL53L0X::writeRegister(const uint8_t reg_address, const uint8_t value) { uint8_t cmd[2] {reg_address, value}; @@ -537,46 +543,29 @@ void VL53L0X::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x29); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *VL53L0X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - VL53L0X *instance = new VL53L0X(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int vl53l0x_main(int argc, char *argv[]) { int ch; using ThisDriver = VL53L0X; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 400000; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.i2c_address = VL53L0X_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = atoi(cli.optarg()); + cli.rotation = (Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/vl53l0x/VL53L0X.hpp b/src/drivers/distance_sensor/vl53l0x/VL53L0X.hpp index c07917d9a0c6..49ebd71c6f50 100644 --- a/src/drivers/distance_sensor/vl53l0x/VL53L0X.hpp +++ b/src/drivers/distance_sensor/vl53l0x/VL53L0X.hpp @@ -54,13 +54,10 @@ class VL53L0X : public device::I2C, public I2CSPIDriver { public: - VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, - int address = VL53L0X_BASEADDR); + VL53L0X(const I2CSPIDriverConfig &config); ~VL53L0X() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); /** @@ -68,17 +65,14 @@ class VL53L0X : public device::I2C, public I2CSPIDriver */ void print_status() override; - /** - * Initialise the automatic measurement state machine and start it. - */ - void start(); - /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ void RunImpl(); + int init() override; + private: int probe() override; diff --git a/src/drivers/distance_sensor/vl53l1x/Kconfig b/src/drivers/distance_sensor/vl53l1x/Kconfig new file mode 100644 index 000000000000..cdb6c003b085 --- /dev/null +++ b/src/drivers/distance_sensor/vl53l1x/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DISTANCE_SENSOR_VL53L1X + bool "vl53l1x" + default n + ---help--- + Enable support for vl53l1x \ No newline at end of file diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp index 28418a0ee2f2..090152d9bc09 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp +++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -36,7 +36,28 @@ #include "vl53l1x.hpp" -#define VL53L1X_SAMPLE_RATE 20 // ms +#define VL53L1X_DELAY 20 //ms +#define VL53L1X_SAMPLE_RATE 200 // ms, default +#define VL53L1X_INTER_MEAS_MS 200 // ms +#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode +#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode +#define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS 13 // region of interest out of bounds error +#define VL53L1X_RANGE_STATUS_OK 0 // range status ok +#define VL53L1X_ROI_FAR_RIGHT 247 // ROI far right of optical center +#define VL53L1X_ROI_MID_RIGHT 215 // ROI middle right of optical center +#define VL53L1X_ROI_CENTER_LEFT 183 // ROI optical center left +#define VL53L1X_ROI_MID_LEFT 167 // ROI middle left of optical center +#define VL53L1X_ROI_FAR_LEFT 151 // ROI far left of optical center +#define VL53L1X_ROI_FAR_RIGHT_LO 10 // ROI far right lo +#define VL53L1X_ROI_MID_RIGHT_LO 42 // ROI middle right of optical center +#define VL53L1X_ROI_CENTER_LEFT_LO 74 // ROI optical center left lo +#define VL53L1X_ROI_MID_LEFT_LO 90 // ROI middle left of optical center +#define VL53L1X_ROI_FAR_LEFT_LO 106 // ROI far left lo +#define VL53L1X_ROI_FAR_RIGHT_HI 243 // ROI far right hi +#define VL53L1X_ROI_MID_RIGHT_HI 211 // ROI middle right of optical center +#define VL53L1X_ROI_CENTER_LEFT_HI 179 // ROI optical center left hi +#define VL53L1X_ROI_MID_LEFT_HI 163 // ROI middle left of optical center +#define VL53L1X_ROI_FAR_LEFT_HI 147 // ROI far left hi /* ST */ const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { @@ -140,16 +161,34 @@ static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, /* end ST */ -VL53L1X::VL53L1X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) : - I2C(DRV_DIST_DEVTYPE_VL53L1X, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_rangefinder(get_device_id(), rotation) +// ROI array assignment + +uint8_t roi_center[] = {VL53L1X_ROI_FAR_RIGHT, VL53L1X_ROI_MID_RIGHT, VL53L1X_ROI_CENTER_LEFT, VL53L1X_ROI_MID_LEFT, VL53L1X_ROI_FAR_LEFT, VL53L1X_ROI_FAR_RIGHT_LO, VL53L1X_ROI_MID_RIGHT_LO, VL53L1X_ROI_CENTER_LEFT_LO, VL53L1X_ROI_MID_LEFT_LO, VL53L1X_ROI_FAR_LEFT_LO, VL53L1X_ROI_FAR_RIGHT_HI, VL53L1X_ROI_MID_RIGHT_HI, VL53L1X_ROI_CENTER_LEFT_HI, VL53L1X_ROI_MID_LEFT_HI, VL53L1X_ROI_FAR_LEFT_HI}; + +VL53L1X::VL53L1X(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_rangefinder(get_device_id(), config.rotation) { - // VL53L1X typical range 0-2 meters with 25 degree field of view + // Set distance mode (1 for ~2m ranging, 2 for ~4m ranging + _distance_mode = VL53L1X_LONG_RANGE; + // VL53L1X typical range 0-4 meters with 27 degree field of view _px4_rangefinder.set_min_distance(0.f); - _px4_rangefinder.set_max_distance(2.f); - _px4_rangefinder.set_fov(math::radians(25.f)); + if (_distance_mode == VL53L1X_SHORT_RANGE) { + _px4_rangefinder.set_max_distance(2.f); + + } else { + _px4_rangefinder.set_max_distance(4.f); + } + + _px4_rangefinder.set_fov(math::radians(27.f)); + + // Zone limits + _zone_limit = sizeof(roi_center) / sizeof(uint8_t); + + // Zone index + _zone_index = 0; // Allow 3 retries as the device typically misses the first measure attempts. I2C::_retries = 3; @@ -166,7 +205,7 @@ VL53L1X::~VL53L1X() int VL53L1X::collect() { uint8_t ret = 0; - uint8_t rangeStatus; + uint8_t rangeStatus = VL53L1X_RANGE_STATUS_OK; uint16_t distance_mm = 0; perf_begin(_sample_perf); @@ -175,7 +214,7 @@ int VL53L1X::collect() ret = VL53L1X_GetRangeStatus(&rangeStatus); - if ((ret != PX4_OK) | (rangeStatus != PX4_OK)) { + if ((ret != PX4_OK) | (rangeStatus == VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS)) { perf_count(_comms_errors); perf_end(_sample_perf); return PX4_ERROR; @@ -215,32 +254,32 @@ int VL53L1X::probe() return -EIO; } + _retries = 1; + return PX4_OK; } void VL53L1X::RunImpl() { uint8_t dataReady = 0; - VL53L1X_CheckForDataReady(&dataReady); if (dataReady == 1) { collect(); } - ScheduleDelayed(VL53L1X_SAMPLE_RATE); -} + ScheduleDelayed(VL53L1X_DELAY); -void VL53L1X::start() -{ - // Schedule the first cycle. - ScheduleNow(); + // zone modulus increment + _zone_index = (_zone_index + 1) % _zone_limit; + + // Set the ROI center based on zone incrementation + VL53L1X_SetROICenter(roi_center[_zone_index]); } int VL53L1X::init() { int ret = PX4_OK; - ret = device::I2C::init(); if (ret != PX4_OK) { @@ -248,9 +287,16 @@ int VL53L1X::init() return PX4_ERROR; } + // Spad width (x) & height (y) + + uint8_t x = 4; + uint8_t y = 4; + ret |= VL53L1X_SensorInit(); - ret |= VL53L1X_ConfigBig(2, VL53L1X_SAMPLE_RATE); - ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_SAMPLE_RATE); + ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE); + ret |= VL53L1X_SetROI(x, y); + ret |= VL53L1X_SetROICenter(roi_center[_zone_index]); + ret |= VL53L1X_SetInterMeasurementInMs(VL53L1X_INTER_MEAS_MS); ret |= VL53L1X_StartRanging(); if (ret != PX4_OK) { @@ -258,21 +304,18 @@ int VL53L1X::init() return PX4_ERROR; } - PX4_INFO("vl53l1x init success"); + PX4_DEBUG("vl53l1x init success"); + ScheduleNow(); return PX4_OK; } -void VL53L1X::stop() -{ - VL53L1X_StopRanging(); -} - void VL53L1X::print_usage() { PRINT_MODULE_USAGE_NAME("vl53l1x", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x29); PRINT_MODULE_USAGE_PARAM_INT('R', 25, 1, 25, "Sensor rotation - downward facing by default", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -488,6 +531,18 @@ int8_t VL53L1X::VL53L1X_StopRanging() return status; } +int8_t VL53L1X::VL53L1X_SetROI(uint16_t x, uint16_t y) +{ + // set ROI size x and y + return VL53L1_WrByte(ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, (y - 1) << 4 | (x - 1)); +} + +int8_t VL53L1X::VL53L1X_SetROICenter(uint8_t zone) +{ + // Set ROI spad center + return VL53L1_WrByte(VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, zone); +} + /** * @brief This function programs the Intermeasurement period in ms\n * Intermeasurement period must be >/= timing budget. @@ -658,42 +713,24 @@ int8_t VL53L1X::VL53L1X_ConfigBig(uint16_t DM, uint16_t TimingBudgetInMs) /* end ST */ -I2CSPIDriverBase *VL53L1X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - VL53L1X *instance = new VL53L1X(iterator.configuredBusOption(), iterator.bus(), cli.orientation, cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - instance->start(); - return instance; -} - extern "C" __EXPORT int vl53l1x_main(int argc, char *argv[]) { int ch; using ThisDriver = VL53L1X; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 400000; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; + cli.i2c_address = VL53L1X_BASEADDR; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.orientation = atoi(cli.optarg()); + cli.rotation = (Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp index 3ac79644f78f..8db68145e020 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp +++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.hpp @@ -94,17 +94,16 @@ /* Configuration Constants */ #define VL53L1X_BASEADDR 0x29 +#define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode +#define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode class VL53L1X : public device::I2C, public I2CSPIDriver { public: - VL53L1X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, - int address = VL53L1X_BASEADDR); + VL53L1X(const I2CSPIDriverConfig &config); ~VL53L1X() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); /** @@ -112,16 +111,6 @@ class VL53L1X : public device::I2C, public I2CSPIDriver */ void print_status() override; - /** - * Initialise the automatic measurement state machine and start it. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - virtual int init() override; /** @@ -130,6 +119,15 @@ class VL53L1X : public device::I2C, public I2CSPIDriver */ void RunImpl(); + // Distance mode member variable + uint16_t _distance_mode{VL53L1X_LONG_RANGE}; + + // Zone index member variable + uint8_t _zone_index{0}; + + // Zone limit member variable + uint8_t _zone_limit{0}; + private: int probe() override; @@ -156,7 +154,8 @@ class VL53L1X : public device::I2C, public I2CSPIDriver // Remove if config store int8_t VL53L1X_ConfigBig(uint16_t DM, uint16_t TimingBudgetInMs); int8_t VL53L1X_SetInterMeasurementInMs(uint32_t InterMeasurementInMs); - + int8_t VL53L1X_SetROICenter(uint8_t data); + int8_t VL53L1X_SetROI(uint16_t x, uint16_t y); PX4Rangefinder _px4_rangefinder; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h deleted file mode 100644 index 763cab352468..000000000000 --- a/src/drivers/drv_airspeed.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * 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 drv_airspeed.h - * - * Airspeed driver interface. - * - * @author Simon Wilks - */ - -#ifndef _DRV_AIRSPEED_H -#define _DRV_AIRSPEED_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define AIRSPEED_BASE_DEVICE_PATH "/dev/airspeed" -#define AIRSPEED0_DEVICE_PATH "/dev/airspeed0" - -/* - * ioctl() definitions - * - * Airspeed drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ - -#define _AIRSPEEDIOCBASE (0x7700) -#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n)) - -#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) - -/** airspeed scaling factors; out = (in * Vscale) + offset */ -struct airspeed_scale { - float offset_pa; - float scale; -}; - -#endif /* _DRV_AIRSPEED_H */ diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 0654b287a151..fb1cc0722f84 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -47,6 +47,10 @@ #include #include +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +#include +#endif + __BEGIN_DECLS /** @@ -76,6 +80,10 @@ typedef struct hrt_call { hrt_abstime period; hrt_callout callout; void *arg; +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + hrt_callout usr_callout; + void *usr_arg; +#endif } *hrt_call_t; @@ -84,6 +92,40 @@ extern const uint16_t latency_bucket_count; extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT]; extern uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +typedef struct latency_info { + uint16_t bucket; + uint32_t counter; +} latency_info_t; + +#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) + +typedef struct hrt_boardctl { + hrt_call_t entry; + hrt_abstime time; /* delay or calltime */ + hrt_abstime interval; + hrt_callout callout; + void *arg; +} hrt_boardctl_t; + +typedef struct latency_boardctl { + uint16_t bucket_idx; + uint16_t counter_idx; + latency_info_t latency; +} latency_boardctl_t; + +#define _HRTIOC(_n) (_PX4_IOC(_HRTIOCBASE, _n)) + +#define HRT_WAITEVENT _HRTIOC(1) +#define HRT_ABSOLUTE_TIME _HRTIOC(2) +#define HRT_CALL_AFTER _HRTIOC(3) +#define HRT_CALL_AT _HRTIOC(4) +#define HRT_CALL_EVERY _HRTIOC(5) +#define HRT_CANCEL _HRTIOC(6) +#define HRT_GET_LATENCY _HRTIOC(7) +#define HRT_RESET_LATENCY _HRTIOC(8) + +#endif + /** * Get absolute time in [us] (does not wrap). */ @@ -92,12 +134,25 @@ __EXPORT extern hrt_abstime hrt_absolute_time(void); /** * Convert a timespec to absolute time. */ -__EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts); +static inline hrt_abstime ts_to_abstime(const struct timespec *ts) +{ + hrt_abstime result; + + result = (hrt_abstime)(ts->tv_sec) * 1000000; + result += ts->tv_nsec / 1000; + + return result; +} /** * Convert absolute time to a timespec. */ -__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) +{ + ts->tv_sec = abstime / 1000000; + abstime -= ts->tv_sec * 1000000; + ts->tv_nsec = abstime * 1000; +} /** * Compute the delta between a timestamp taken in the past @@ -110,15 +165,6 @@ static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then) return hrt_absolute_time() - *then; } -/** - * Compute the delta between a timestamp taken in the past - * and now. - * - * This function is safe to use even if the timestamp is updated - * by an interrupt during execution. - */ -__EXPORT extern hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then); - /** * Store the absolute time in an interrupt-safe fashion. * @@ -208,8 +254,36 @@ static inline void px4_lockstep_progress(int component) { } static inline void px4_lockstep_wait_for_components(void) { } #endif /* defined(ENABLE_LOCKSTEP_SCHEDULER) */ -__END_DECLS +/* Latency counter functions */ + +static inline uint16_t get_latency_bucket_count(void) { return LATENCY_BUCKET_COUNT; } + +#if defined(CONFIG_BUILD_FLAT) || !defined(__PX4_NUTTX) + +static inline latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx) +{ + latency_info_t ret = {latency_buckets[bucket_idx], latency_counters[counter_idx]}; + return ret; +} + +static inline void reset_latency_counters(void) +{ + for (int i = 0; i <= get_latency_bucket_count(); i++) { + latency_counters[i] = 0; + } +} + +#else + +/* NuttX protected/kernel build interface functions */ + +latency_info_t get_latency(uint16_t bucket_idx, uint16_t counter_idx); +void reset_latency_counters(void); + +#endif + +__END_DECLS #ifdef __cplusplus diff --git a/src/drivers/drv_input_capture.h b/src/drivers/drv_input_capture.h index dc79e14612d8..b90711ff898e 100644 --- a/src/drivers/drv_input_capture.h +++ b/src/drivers/drv_input_capture.h @@ -48,26 +48,10 @@ __BEGIN_DECLS -/** - * Path for the default capture input device. - * - * - */ -#define INPUT_CAPTURE_BASE_DEVICE_PATH "/dev/capture" -#define INPUT_CAPTURE0_DEVICE_PATH "/dev/capture0" - typedef void (*capture_callback_t)(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); -/** - * Maximum number of PWM input channels supported by the device. - */ -#ifndef INPUT_CAPTURE_MAX_CHANNELS -#define INPUT_CAPTURE_MAX_CHANNELS 6 -#endif - typedef uint16_t capture_filter_t; -typedef uint16_t capture_t; typedef enum input_capture_edge { Disabled = 0, @@ -76,104 +60,14 @@ typedef enum input_capture_edge { Both = 3 } input_capture_edge; -typedef struct input_capture_element_t { - hrt_abstime time_stamp; - input_capture_edge edge; - bool overrun; -} input_capture_element_t; - typedef struct input_capture_stats_t { - uint32_t chan_in_edges_out; + uint32_t edges; uint32_t overflows; uint32_t last_edge; hrt_abstime last_time; - uint16_t latnecy; + uint16_t latency; } input_capture_stats_t; -/** - * input capture values for a channel - * - * This allows for Capture input driver values to be set without a - * param_get() dependency - */ -typedef struct input_capture_config_t { - uint8_t channel; - capture_filter_t filter; - input_capture_edge edge; - capture_callback_t callback; - void *context; - -} input_capture_config_t; - -/* - * ioctl() definitions - * - * Note that ioctls and ORB updates should not be mixed, as the - * behaviour of the system in this case is not defined. - */ -#define _INPUT_CAP_BASE 0x2d00 - -/** Set Enable a channel arg is pointer to input_capture_config - * with all parameters set. - * edge controls the mode: Disable will free the capture channel. - * (When edge is Disabled call back and context are ignored) - * context may be null. If callback and context are null the - * callback will be disabled. - * */ - -#define INPUT_CAP_SET _PX4_IOC(_INPUT_CAP_BASE, 0) - -/** Set the call back on a capture channel - arg is pointer to - * input_capture_config with channel call back and context set - * context may be null. If both ate null the call back will be - * disabled */ -#define INPUT_CAP_SET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 1) - -/** Get the call back on a capture channel - arg is pointer to - * input_capture_config with channel set. - */ -#define INPUT_CAP_GET_CALLBACK _PX4_IOC(_INPUT_CAP_BASE, 2) - -/** Set Edge a channel arg is pointer to input_capture_config - * with channel and edge set */ -#define INPUT_CAP_SET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 3) - -/** Get Edge for a channel arg is pointer to input_capture_config - * with channel set */ -#define INPUT_CAP_GET_EDGE _PX4_IOC(_INPUT_CAP_BASE, 4) - -/** Set Filter input filter channel arg is pointer to input_capture_config - * with channel and filter set */ -#define INPUT_CAP_SET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 5) - -/** Set Filter input filter channel arg is pointer to input_capture_config - * with channel set */ -#define INPUT_CAP_GET_FILTER _PX4_IOC(_INPUT_CAP_BASE, 6) - -/** Get the number of capture in *(unsigned *)arg */ -#define INPUT_CAP_GET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 7) - -/** Set the number of capture in (unsigned)arg - allows change of - * split between servos and capture */ -#define INPUT_CAP_SET_COUNT _PX4_IOC(_INPUT_CAP_BASE, 8) - -/** Get channel stats - arg is pointer to input_capture_config - * with channel set. - */ -#define INPUT_CAP_GET_STATS _PX4_IOC(_INPUT_CAP_BASE, 9) - -/** Get channel stats - arg is pointer to input_capture_config - * with channel set. - */ -#define INPUT_CAP_GET_CLR_STATS _PX4_IOC(_INPUT_CAP_BASE, 10) - -/* - * - * - * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE! - * - * - */ __EXPORT int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filter_t filter, capture_callback_t callback, void *context); diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 821390e435ff..d6096d1f5241 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -86,6 +86,11 @@ struct pwm_output_values { */ #define PWM_MOTOR_OFF 900 +/** + * Default value for a servo stop + */ +#define PWM_SERVO_STOP 1500 + /** * Default minimum PWM in us */ @@ -99,7 +104,7 @@ struct pwm_output_values { /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2150 +#define PWM_HIGHEST_MAX 2500 /** * Default maximum PWM in us @@ -170,15 +175,9 @@ typedef uint16_t servo_position_t; /** start DSM bind */ #define DSM_BIND_START _PX4_IOC(_PWM_SERVO_BASE, 10) -/** set the PWM value for failsafe */ -#define PWM_SERVO_SET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 12) - /** get the PWM value for failsafe */ #define PWM_SERVO_GET_FAILSAFE_PWM _PX4_IOC(_PWM_SERVO_BASE, 13) -/** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 14) - /** get the PWM value when disarmed */ #define PWM_SERVO_GET_DISARMED_PWM _PX4_IOC(_PWM_SERVO_BASE, 15) @@ -194,53 +193,6 @@ typedef uint16_t servo_position_t; /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19) -/** get the TRIM value the output will send */ -#define PWM_SERVO_GET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 21) - -/** set the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_SET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 23) - -/** get the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_GET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 24) - -/** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 25) - -/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 26) - -/** make failsafe non-recoverable (termination) if it occurs */ -#define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 27) - -/** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28) - -/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ -#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32) - -/** set auxillary output mode. These correspond to enum Mode in px4fmu/fmu.cpp */ -#define PWM_SERVO_MODE_NONE 0 -#define PWM_SERVO_MODE_1PWM 1 -#define PWM_SERVO_MODE_2PWM 2 -#define PWM_SERVO_MODE_2PWM2CAP 3 -#define PWM_SERVO_MODE_3PWM 4 -#define PWM_SERVO_MODE_3PWM1CAP 5 -#define PWM_SERVO_MODE_4PWM 6 -#define PWM_SERVO_MODE_4PWM1CAP 7 -#define PWM_SERVO_MODE_4PWM2CAP 8 -#define PWM_SERVO_MODE_5PWM 9 -#define PWM_SERVO_MODE_5PWM1CAP 10 -#define PWM_SERVO_MODE_6PWM 11 -#define PWM_SERVO_MODE_8PWM 12 -#define PWM_SERVO_MODE_12PWM 13 -#define PWM_SERVO_MODE_14PWM 14 -#define PWM_SERVO_MODE_4CAP 15 -#define PWM_SERVO_MODE_5CAP 16 -#define PWM_SERVO_MODE_6CAP 17 -#define PWM_SERVO_ENTER_TEST_MODE 18 -#define PWM_SERVO_EXIT_TEST_MODE 19 -#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34) - /* * * @@ -249,9 +201,6 @@ typedef uint16_t servo_position_t; * */ -/** set a single servo to a specific value */ -#define PWM_SERVO_SET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x30 + _servo) - /** get a single specific servo value */ #define PWM_SERVO_GET(_servo) _PX4_IOC(_PWM_SERVO_BASE, 0x50 + _servo) @@ -291,7 +240,7 @@ typedef enum { DShot_cmd_led4_off, // BLHeli32 only DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off - DShot_cmd_signal_line_telemeetry_disable = 32, + DShot_cmd_signal_line_telemetry_disable = 32, DShot_cmd_signal_line_continuous_erpm_telemetry = 33, DShot_cmd_MAX = 47, // >47 are throttle values DShot_cmd_MIN_throttle = 48, @@ -310,15 +259,22 @@ 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. - * @return OK on success. + * as GPIOs or as another function. Already used channels/timers + * will not be configured as PWM. + * @return <0 on error, the initialized channels mask. */ __EXPORT extern int up_pwm_servo_init(uint32_t channel_mask); /** * De-initialise the PWM servo outputs. + * + * @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. + * A value of 0 is ALL channels + * */ -__EXPORT extern void up_pwm_servo_deinit(void); +__EXPORT extern void up_pwm_servo_deinit(uint32_t channel_mask); /** * Arm or disarm servo outputs. @@ -330,8 +286,14 @@ __EXPORT extern void up_pwm_servo_deinit(void); * * @param armed If true, outputs are armed; if false they * are disarmed. + * + * @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. + * A value of 0 is ALL channels + * */ -__EXPORT extern void up_pwm_servo_arm(bool armed); +__EXPORT extern void up_pwm_servo_arm(bool armed, uint32_t channel_mask); /** * Set the servo update rate for all rate groups. @@ -366,7 +328,7 @@ __EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned * Nothing is done if not in oneshot mode. * */ -__EXPORT extern void up_pwm_update(void); +__EXPORT extern void up_pwm_update(unsigned channel_mask); /** * Set the current output value for a channel. @@ -390,12 +352,17 @@ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); * * @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. + * 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 - * @return OK on success. + * @return <0 on error, the initialized channels mask. */ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); +/** + * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) + */ +__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry); + /** * Set the current dshot throttle value for a channel (motor). * @@ -403,7 +370,10 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq * @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE]. * @param telemetry If true, request telemetry from that motor */ -__EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry); +static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) +{ + dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry); +} /** * Send DShot command to a channel (motor). @@ -412,7 +382,11 @@ __EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle * @param command dshot_command_t * @param telemetry If true, request telemetry from that motor */ -__EXPORT extern void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry); +static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) +{ + dshot_motor_data_set(channel, command, telemetry); +} + /** * Trigger dshot data transfer. diff --git a/src/drivers/drv_pwm_trigger.h b/src/drivers/drv_pwm_trigger.h index f066e3750bd1..2abbc7d9c05b 100644 --- a/src/drivers/drv_pwm_trigger.h +++ b/src/drivers/drv_pwm_trigger.h @@ -49,8 +49,9 @@ __BEGIN_DECLS * * @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. - * @return OK on success. + * as GPIOs or as another function. Already used channels/timers + * will not be configured as PWM. + * @return <0 on error, the initialized channels mask. */ __EXPORT extern int up_pwm_trigger_init(uint32_t channel_mask); @@ -78,4 +79,4 @@ __EXPORT extern void up_pwm_trigger_arm(bool armed); */ __EXPORT extern int up_pwm_trigger_set(unsigned channel, uint16_t value); -__END_DECLS \ No newline at end of file +__END_DECLS diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h deleted file mode 100644 index 6ab1d0b49f3c..000000000000 --- a/src/drivers/drv_rc_input.h +++ /dev/null @@ -1,80 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 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 drv_rc_input.h - * - * R/C input interface. - */ - -#ifndef _DRV_RC_INPUT_H -#define _DRV_RC_INPUT_H - -#include -#include -#include - -#include "drv_orb_dev.h" - -/** - * Path for the default R/C input device. - * - * Note that on systems with more than one R/C input path (e.g. - * PX4FMU with PX4IO connected) there may be other devices that - * respond to this protocol. - * - * Input data may be obtained by subscribing to the input_rc - * object, or by poll/reading from the device. - */ -#define RC_INPUT0_DEVICE_PATH "/dev/input_rc0" - -/** - * Maximum RSSI value - */ -#define RC_INPUT_RSSI_MAX 100 - -/** - * Input signal type, value is a control position from zero to 100 - * percent. - */ -typedef uint16_t rc_input_t; - -#define _RC_INPUT_BASE 0x2b00 - -/** Enable RSSI input via ADC */ -#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) - -/** Enable RSSI input via PWM signal */ -#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) - -#endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1af334cb74b9..4547f28cec27 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -51,7 +51,7 @@ */ #define DRV_MAG_DEVTYPE_HMC5883 0x01 -#define DRV_MAG_DEVTYPE_LSM303D_LEGACY 0x02 + #define DRV_MAG_DEVTYPE_MAGSIM 0x03 #define DRV_MAG_DEVTYPE_AK8963 0x04 #define DRV_MAG_DEVTYPE_LIS3MDL 0x05 @@ -59,15 +59,16 @@ #define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_MAG_DEVTYPE_QMC5883L 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 +#define DRV_MAG_DEVTYPE_VCM1193L 0x0A #define DRV_MAG_DEVTYPE_IST8308 0x0B #define DRV_MAG_DEVTYPE_LIS2MDL 0x0C #define DRV_IMU_DEVTYPE_LSM303D 0x11 -#define DRV_ACC_DEVTYPE_MPU6000_LEGACY 0x13 #define DRV_IMU_DEVTYPE_SIM 0x14 -#define DRV_ACC_DEVTYPE_MPU9250_LEGACY 0x16 +#define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15 +#define DRV_FLOW_DEVTYPE_SIM 0x16 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 @@ -77,18 +78,19 @@ #define DRV_IMU_DEVTYPE_ICM40609D 0x27 #define DRV_IMU_DEVTYPE_ICM20948 0x28 #define DRV_IMU_DEVTYPE_ICM42605 0x29 +#define DRV_IMU_DEVTYPE_ICM42670P 0x2A #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 #define DRV_ACC_DEVTYPE_MPU6050 0x33 -#define DRV_ACC_DEVTYPE_MPU6500_LEGACY 0x34 + #define DRV_GYR_DEVTYPE_MPU6050 0x35 #define DRV_IMU_DEVTYPE_MPU6500 0x36 -#define DRV_ACC_DEVTYPE_ICM20602_LEGACY 0x37 + #define DRV_IMU_DEVTYPE_ICM20602 0x38 -#define DRV_ACC_DEVTYPE_ICM20608_LEGACY 0x39 + #define DRV_IMU_DEVTYPE_ICM20608G 0x3A -#define DRV_ACC_DEVTYPE_ICM20689_LEGACY 0x3B + #define DRV_IMU_DEVTYPE_ICM20689 0x3C #define DRV_BARO_DEVTYPE_MS5611 0x3D #define DRV_BARO_DEVTYPE_MS5607 0x3E @@ -99,14 +101,19 @@ #define DRV_MAG_DEVTYPE_BMM150 0x43 #define DRV_IMU_DEVTYPE_ST_LSM9DS1_AG 0x44 #define DRV_MAG_DEVTYPE_ST_LSM9DS1_M 0x45 -#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x46 -#define DRV_DIFF_PRESS_DEVTYPE_MS4525 0x47 -#define DRV_DIFF_PRESS_DEVTYPE_MS5525 0x48 -#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x49 -#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4A -#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4B + +#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x46 +#define DRV_DIFF_PRESS_DEVTYPE_MS4515 0x47 +#define DRV_DIFF_PRESS_DEVTYPE_MS4525DO 0x48 +#define DRV_DIFF_PRESS_DEVTYPE_MS5525DSO 0x49 +#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x4A +#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4B +#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4C + #define DRV_BARO_DEVTYPE_LPS33HW 0x4C #define DRV_BARO_DEVTYPE_TCBP001TA 0x4D +#define DRV_BARO_DEVTYPE_MS5837 0x4E +#define DRV_BARO_DEVTYPE_SPL06 0x4F #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 @@ -117,6 +124,7 @@ #define DRV_IMU_DEVTYPE_ADIS16470 0x58 #define DRV_IMU_DEVTYPE_ADIS16477 0x59 +#define DRV_BARO_DEVTYPE_MPC2520 0x5F #define DRV_BARO_DEVTYPE_LPS22HB 0x60 #define DRV_ACC_DEVTYPE_LSM303AGR 0x61 @@ -126,12 +134,10 @@ #define DRV_GYR_DEVTYPE_BMI088 0x66 #define DRV_BARO_DEVTYPE_BMP388 0x67 #define DRV_BARO_DEVTYPE_DPS310 0x68 +#define DRV_PWM_DEVTYPE_PCA9685 0x69 #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b -#define DRV_FLOW_DEVTYPE_PMW3901 0x6c -#define DRV_FLOW_DEVTYPE_PAW3902 0x6d -#define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e -#define DRV_PWM_DEVTYPE_PCA9685 0x6f + #define DRV_DIST_DEVTYPE_LL40LS 0x70 #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 @@ -141,7 +147,8 @@ #define DRV_DIST_DEVTYPE_TERARANGER 0x75 #define DRV_DIST_DEVTYPE_VL53L0X 0x76 #define DRV_POWER_DEVTYPE_INA226 0x77 -#define DRV_POWER_DEVTYPE_VOXLPM 0x78 +#define DRV_POWER_DEVTYPE_INA228 0x78 +#define DRV_POWER_DEVTYPE_VOXLPM 0x79 #define DRV_LED_DEVTYPE_RGBLED 0x7a #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b @@ -162,20 +169,49 @@ #define DRV_MAG_DEVTYPE_UAVCAN 0x88 #define DRV_DIST_DEVTYPE_UAVCAN 0x89 -#define DRV_ADC_DEVTYPE_ADS1115 0x90 -#define DRV_DIST_DEVTYPE_VL53L1X 0x91 -#define DRV_DIST_DEVTYPE_CM8JL65 0x92 -#define DRV_DIST_DEVTYPE_LEDDARONE 0x93 -#define DRV_DIST_DEVTYPE_MAVLINK 0x94 -#define DRV_DIST_DEVTYPE_PGA460 0x95 -#define DRV_DIST_DEVTYPE_PX4FLOW 0x96 -#define DRV_DIST_DEVTYPE_TFMINI 0x97 -#define DRV_DIST_DEVTYPE_ULANDING 0x98 - -#define DRV_GPIO_DEVTYPE_MCP23009 0x99 - -#define DRV_DIST_DEVTYPE_SIM 0x9a -#define DRV_DIST_DEVTYPE_SRF05 0x9b +#define DRV_ADC_DEVTYPE_ADS1115 0x90 + +#define DRV_DIST_DEVTYPE_VL53L1X 0x91 +#define DRV_DIST_DEVTYPE_CM8JL65 0x92 +#define DRV_DIST_DEVTYPE_LEDDARONE 0x93 +#define DRV_DIST_DEVTYPE_MAVLINK 0x94 +#define DRV_DIST_DEVTYPE_PGA460 0x95 +#define DRV_DIST_DEVTYPE_PX4FLOW 0x96 +#define DRV_DIST_DEVTYPE_TFMINI 0x97 +#define DRV_DIST_DEVTYPE_ULANDING 0x98 +#define DRV_DIST_DEVTYPE_AFBRS50 0x99 +#define DRV_DIST_DEVTYPE_SIM 0x9A +#define DRV_DIST_DEVTYPE_SRF05 0x9B +#define DRV_DIST_DEVTYPE_GY_US42 0x9C + +#define DRV_BAT_DEVTYPE_BATMON_SMBUS 0x9d +#define DRV_POWER_DEVTYPE_INA238 0x9E +#define DRV_GPIO_DEVTYPE_MCP23009 0x9F + +#define DRV_GPS_DEVTYPE_ASHTECH 0xA0 +#define DRV_GPS_DEVTYPE_EMLID_REACH 0xA1 +#define DRV_GPS_DEVTYPE_FEMTOMES 0xA2 +#define DRV_GPS_DEVTYPE_MTK 0xA3 +#define DRV_GPS_DEVTYPE_SBF 0xA4 +#define DRV_GPS_DEVTYPE_UBX 0xA5 +#define DRV_GPS_DEVTYPE_UBX_6 0xA6 +#define DRV_GPS_DEVTYPE_UBX_7 0xA7 +#define DRV_GPS_DEVTYPE_UBX_8 0xA8 +#define DRV_GPS_DEVTYPE_UBX_9 0xA9 +#define DRV_GPS_DEVTYPE_UBX_F9P 0xAA +#define DRV_GPS_DEVTYPE_NMEA 0xAB + +#define DRV_GPS_DEVTYPE_SIM 0xAF + +#define DRV_TRNS_DEVTYPE_MXS 0xB0 + +#define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 + +#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2 +#define DRV_FLOW_DEVTYPE_PMW3901 0xB3 +#define DRV_FLOW_DEVTYPE_PAW3902 0xB4 +#define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5 +#define DRV_FLOW_DEVTYPE_PAA3905 0xB6 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/drv_watchdog.h b/src/drivers/drv_watchdog.h index 9baeeb063d09..129eee0aa679 100644 --- a/src/drivers/drv_watchdog.h +++ b/src/drivers/drv_watchdog.h @@ -45,5 +45,6 @@ __BEGIN_DECLS void watchdog_init(void); +void watchdog_init_ex(int prescale, int reload); // Optional interface void watchdog_pet(void); __END_DECLS diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 177629738c30..757a7a8d2499 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 @@ -30,10 +30,18 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + px4_add_module( MODULE drivers__dshot MAIN dshot - STACK_MAIN 1200 + COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" SRCS DShot.cpp DShotTelemetry.cpp @@ -42,7 +50,6 @@ px4_add_module( arch_dshot mixer mixer_module - output_limit MODULE_CONFIG module.yaml ) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 42449567aca1..122cfa391e9e 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -33,6 +33,10 @@ #include "DShot.h" +#include + +#include + char DShot::_telemetry_device[] {}; px4::atomic_bool DShot::_request_telemetry_init{false}; @@ -44,6 +48,10 @@ DShot::DShot() : _mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE); _mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE); + if (_mixing_output.useDynamicMixing()) { + // Avoid using the PWM failsafe params + _mixing_output.setAllFailsafeValues(UINT16_MAX); + } } DShot::~DShot() @@ -78,6 +86,8 @@ int DShot::init() _mixing_output.setDriverInstance(_class_instance); + _output_mask = (1u << _num_outputs) - 1; + // Getting initial parameter values update_params(); @@ -86,186 +96,6 @@ int DShot::init() return OK; } -int DShot::set_mode(const Mode mode) -{ - unsigned old_mask = _output_mask; - - /* - * Configure for output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_1PWM: - // default output rates - _output_mask = 0x1; - _outputs_initialized = false; - _num_outputs = 1; - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM - up_input_capture_set(2, Rising, 0, NULL, NULL); - up_input_capture_set(3, Rising, 0, NULL, NULL); - PX4_DEBUG("MODE_2PWM2CAP"); -#endif - - // FALLTHROUGH - - case MODE_2PWM: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_2PWM"); - - // default output rates - _output_mask = 0x3; - _outputs_initialized = false; - _num_outputs = 2; - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_3PWM1CAP"); - up_input_capture_set(3, Rising, 0, NULL, NULL); -#endif - - // FALLTHROUGH - - case MODE_3PWM: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_3PWM"); - - // default output rates - _output_mask = 0x7; - _outputs_initialized = false; - _num_outputs = 3; - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_4PWM1CAP: - PX4_DEBUG("MODE_4PWM1CAP"); - up_input_capture_set(4, Rising, 0, NULL, NULL); -#endif - - // FALLTHROUGH - - case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs - PX4_DEBUG("MODE_4PWM"); - - // default output rates - _output_mask = 0xf; - _outputs_initialized = false; - _num_outputs = 4; - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_4PWM2CAP: - PX4_DEBUG("MODE_4PWM2CAP"); - up_input_capture_set(5, Rising, 0, NULL, NULL); - - // default output rates - _output_mask = 0x0f; - _outputs_initialized = false; - _num_outputs = 4; - - break; -#endif - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_5PWM1CAP: - PX4_DEBUG("MODE_5PWM1CAP"); - up_input_capture_set(5, Rising, 0, NULL, NULL); -#endif - - // FALLTHROUGH - - case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs - PX4_DEBUG("MODE_5PWM"); - - // default output rates - _output_mask = 0x1f; - _outputs_initialized = false; - _num_outputs = 5; - - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case MODE_6PWM: - PX4_DEBUG("MODE_6PWM"); - - // default output rates - _output_mask = 0x3f; - _outputs_initialized = false; - _num_outputs = 6; - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case MODE_8PWM: // AeroCore PWMs as 8 PWM outs - PX4_DEBUG("MODE_8PWM"); - // default output rates - _output_mask = 0xff; - _outputs_initialized = false; - _num_outputs = 8; - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case MODE_12PWM: - PX4_DEBUG("MODE_12PWM"); - // default output rates - _output_mask = 0xfff; - _outputs_initialized = false; - _num_outputs = 12; - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case MODE_14PWM: - PX4_DEBUG("MODE_14PWM"); - // default output rates - _output_mask = 0x3fff; - _outputs_initialized = false; - _num_outputs = 14; - - break; -#endif - - case MODE_NONE: - PX4_DEBUG("MODE_NONE"); - _output_mask = 0x0; - _outputs_initialized = false; - _num_outputs = 0; - - if (old_mask != _output_mask) { - // disable motor outputs - enable_dshot_outputs(false); - } - - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - int DShot::task_spawn(int argc, char *argv[]) { DShot *instance = new DShot(); @@ -289,53 +119,113 @@ int DShot::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -void DShot::capture_trampoline(void *context, const uint32_t channel_index, const hrt_abstime edge_time, - const uint32_t edge_state, const uint32_t overflow) +void DShot::enable_dshot_outputs(const bool enabled) { - DShot *dev = static_cast(context); - dev->capture_callback(channel_index, edge_time, edge_state, overflow); -} + if (enabled && !_outputs_initialized) { + if (_mixing_output.useDynamicMixing()) { -void DShot::capture_callback(const uint32_t channel_index, const hrt_abstime edge_time, - const uint32_t edge_state, const uint32_t overflow) -{ - fprintf(stdout, "DShot: Capture chan:%d time:%lld state:%d overflow:%d\n", channel_index, edge_time, edge_state, - overflow); -} + unsigned int dshot_frequency = 0; + uint32_t dshot_frequency_param = 0; -void DShot::enable_dshot_outputs(const bool enabled) -{ - if (enabled && !_outputs_initialized && _output_mask != 0) { - DShotConfig config = (DShotConfig)_param_dshot_config.get(); + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = io_timer_get_group(timer); - unsigned int dshot_frequency = DSHOT600; + if (channels == 0) { + continue; + } - switch (config) { - case DShotConfig::DShot150: - dshot_frequency = DSHOT150; - break; + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - case DShotConfig::DShot300: - dshot_frequency = DSHOT300; - break; + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + unsigned int dshot_frequency_request = 0; - case DShotConfig::DShot600: - dshot_frequency = DSHOT600; - break; + if (tim_config == -5) { + dshot_frequency_request = DSHOT150; - case DShotConfig::DShot1200: - dshot_frequency = DSHOT1200; - break; + } else if (tim_config == -4) { + dshot_frequency_request = DSHOT300; - default: - break; - } + } else if (tim_config == -3) { + dshot_frequency_request = DSHOT600; - int ret = up_dshot_init(_output_mask, dshot_frequency); + } else if (tim_config == -2) { + dshot_frequency_request = DSHOT1200; - if (ret != 0) { - PX4_ERR("up_dshot_init failed (%i)", ret); - return; + } else { + _output_mask &= ~channels; // don't use for dshot + } + + if (dshot_frequency_request != 0) { + if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { + PX4_WARN("Only supporting a single frequency, adjusting param %s", param_name); + param_set_no_notification(handle, &dshot_frequency_param); + + } else { + dshot_frequency = dshot_frequency_request; + dshot_frequency_param = tim_config; + } + } + } + + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _output_mask = ret; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _output_mask) == 0) { + _mixing_output.disableFunction(i); + } + } + + if (_output_mask == 0) { + // exit the module if no outputs used + request_stop(); + return; + } + + } else { + DShotConfig config = (DShotConfig)_param_dshot_config.get(); + + unsigned int dshot_frequency = DSHOT600; + + switch (config) { + case DShotConfig::DShot150: + dshot_frequency = DSHOT150; + break; + + case DShotConfig::DShot300: + dshot_frequency = DSHOT300; + break; + + case DShotConfig::DShot600: + dshot_frequency = DSHOT600; + break; + + case DShotConfig::DShot1200: + dshot_frequency = DSHOT1200; + break; + + default: + break; + } + + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret < 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _output_mask = ret; } _outputs_initialized = true; @@ -355,8 +245,18 @@ void DShot::update_telemetry_num_motors() int motor_count = 0; - if (_mixing_output.mixers()) { - motor_count = _mixing_output.mixers()->get_multirotor_count(); + if (_mixing_output.useDynamicMixing()) { + for (unsigned i = 0; i < _num_outputs; ++i) { + if (_mixing_output.isFunctionSet(i)) { + _telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i); + ++motor_count; + } + } + + } else { + if (_mixing_output.mixers()) { + motor_count = _mixing_output.mixers()->get_multirotor_count(); + } } _telemetry->handler.setNumMotors(motor_count); @@ -373,6 +273,8 @@ void DShot::init_telemetry(const char *device) } } + _telemetry->esc_status_pub.advertise(); + int ret = _telemetry->handler.init(device); if (ret != 0) { @@ -382,24 +284,26 @@ void DShot::init_telemetry(const char *device) update_telemetry_num_motors(); } -void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data) +void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) { // fill in new motor data esc_status_s &esc_status = _telemetry->esc_status_pub.get(); - if (motor_index < esc_status_s::CONNECTED_ESC_MAX) { - esc_status.esc_online_flags |= 1 << motor_index; + if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) { + esc_status.esc_online_flags |= 1 << telemetry_index; - esc_status.esc[motor_index].timestamp = data.time; - esc_status.esc[motor_index].esc_rpm = (static_cast(data.erpm) * 100) / (_param_mot_pole_count.get() / 2); - esc_status.esc[motor_index].esc_voltage = static_cast(data.voltage) * 0.01f; - esc_status.esc[motor_index].esc_current = static_cast(data.current) * 0.01f; - esc_status.esc[motor_index].esc_temperature = data.temperature; + esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + esc_status.esc[telemetry_index].timestamp = data.time; + esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / + (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f; + esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f; + esc_status.esc[telemetry_index].esc_temperature = static_cast(data.temperature); // TODO: accumulate consumption and use for battery estimation } // publish when motor index wraps (which is robust against motor timeouts) - if (motor_index <= _telemetry->last_motor_index) { + if (telemetry_index <= _telemetry->last_telemetry_index) { esc_status.timestamp = hrt_absolute_time(); esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_count = _telemetry->handler.numMotors(); @@ -415,7 +319,7 @@ void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetr esc_status.esc_online_flags = 0; } - _telemetry->last_motor_index = motor_index; + _telemetry->last_telemetry_index = telemetry_index; } int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index) @@ -433,9 +337,18 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num cmd.num_repetitions = num_repetitions; _new_command.store(&cmd); + hrt_abstime timestamp_for_timeout = hrt_absolute_time(); + // wait until main thread processed it while (_new_command.load()) { - px4_usleep(1000); + + if (hrt_elapsed_time(×tamp_for_timeout) < 2_s) { + px4_usleep(1000); + + } else { + _new_command.store(nullptr); + PX4_WARN("DShot command timeout!"); + } } return 0; @@ -481,6 +394,7 @@ int DShot::request_esc_info() _current_command.motor_mask = 1 << motor_index; _current_command.num_repetitions = 1; _current_command.command = DShot_cmd_esc_info; + _current_command.save = false; PX4_DEBUG("Requesting ESC info for motor %i", motor_index); return motor_index; @@ -513,6 +427,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], if (stop_motors) { + int telemetry_index = 0; + // when motors are stopped we check if we have other commands to send for (int i = 0; i < (int)num_outputs; i++) { if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) { @@ -520,30 +436,75 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], up_dshot_motor_command(i, _current_command.command, true); } else { - up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); } + + telemetry_index += _mixing_output.isFunctionSet(i); } if (_current_command.valid()) { --_current_command.num_repetitions; + + if (_current_command.num_repetitions == 0 && _current_command.save) { + _current_command.save = false; + _current_command.num_repetitions = 10; + _current_command.command = dshot_command_t::DShot_cmd_save_settings; + } } } else { + int telemetry_index = 0; + for (int i = 0; i < (int)num_outputs; i++) { - if (outputs[i] == DSHOT_DISARM_VALUE) { - up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + + uint16_t output = outputs[i]; + + if (output == DSHOT_DISARM_VALUE) { + up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index); } else { - up_dshot_motor_data_set(i, math::min(outputs[i], static_cast(DSHOT_MAX_THROTTLE)), - i == requested_telemetry_index); + + // DShot 3D splits the throttle ranges in two. + // This is in terms of DShot values, code below is in terms of actuator_output + // Direction 1) 48 is the slowest, 1047 is the fastest. + // Direction 2) 1049 is the slowest, 2047 is the fastest. + if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) { + if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) { + output = DSHOT_DISARM_VALUE; + + } else { + bool upper_range = output >= 1000; + + if (upper_range) { + output -= 1000; + + } else { + output = 999 - output; // lower range is inverted + } + + float max_output = 999.f; + float min_output = max_output * _param_dshot_min.get(); + output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output)); + + if (upper_range) { + output += 1000; + } + + } + } + + up_dshot_motor_data_set(i, math::min(output, static_cast(DSHOT_MAX_THROTTLE)), + telemetry_index == requested_telemetry_index); } + + telemetry_index += _mixing_output.isFunctionSet(i); } // clear commands when motors are running _current_command.clear(); } - if (stop_motors || num_control_groups_updated > 0) { + if (stop_motors || num_control_groups_updated > 0 || _mixing_output.useDynamicMixing()) { up_dshot_trigger(); } @@ -560,12 +521,14 @@ void DShot::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); _mixing_output.update(); // update output status if armed or if mixer is loaded - bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers(); + bool outputs_on = _mixing_output.armed().armed || _mixing_output.initialized(); if (_outputs_on != outputs_on) { enable_dshot_outputs(outputs_on); @@ -606,538 +569,153 @@ void DShot::Run() } } - // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) - _mixing_output.updateSubscriptions(true); - - perf_end(_cycle_perf); -} - -void DShot::update_params() -{ - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - updateParams(); - - // we use a minimum value of 1, since 0 is for disarmed - _mixing_output.setAllMinValues(math::constrain(static_cast((_param_dshot_min.get() * - static_cast(DSHOT_MAX_THROTTLE))), - DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE)); -} - -int DShot::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret; - - // try it as a Capture ioctl next - ret = capture_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) { - return ret; - } - - // if we are in valid PWM mode, try it as a PWM ioctl as well - switch (_mode) { - case MODE_1PWM: - case MODE_2PWM: - case MODE_3PWM: - case MODE_4PWM: - case MODE_5PWM: - case MODE_2PWM2CAP: - case MODE_3PWM1CAP: - case MODE_4PWM1CAP: - case MODE_4PWM2CAP: - case MODE_5PWM1CAP: -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - case MODE_6PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - case MODE_8PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - case MODE_12PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - case MODE_14PWM: -#endif - ret = pwm_ioctl(filp, cmd, arg); - break; + handle_vehicle_commands(); - default: - PX4_DEBUG("not in a PWM mode"); - break; + if (!_mixing_output.armed().armed) { + if (_reversible_outputs != _mixing_output.reversibleOutputs()) { + _reversible_outputs = _mixing_output.reversibleOutputs(); + update_params(); + } } - // if nobody wants it, let CDev have it - if (ret == -ENOTTY) { - ret = CDev::ioctl(filp, cmd, arg); - } + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true); - return ret; + perf_end(_cycle_perf); } -int DShot::pwm_ioctl(file *filp, const int cmd, const unsigned long arg) +void DShot::handle_vehicle_commands() { - int ret = OK; - - PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg); - - lock(); - - switch (cmd) { - case PWM_SERVO_GET_COUNT: - switch (_mode) { + vehicle_command_s vehicle_command; -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 + while (!_current_command.valid() && _vehicle_command_sub.update(&vehicle_command)) { - case MODE_14PWM: - *(unsigned *)arg = 14; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR) { + int function = (int)(vehicle_command.param5 + 0.5); - case MODE_12PWM: - *(unsigned *)arg = 12; - break; -#endif + if (function < 1000) { + const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION + const int first_servo_function = 33; -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + if (function >= first_motor_function && function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) { + function = function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1; - case MODE_8PWM: - *(unsigned *)arg = 8; - break; -#endif + } else if (function >= first_servo_function && function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) { + function = function - first_servo_function + actuator_test_s::FUNCTION_SERVO1; -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + } else { + function = INT32_MAX; + } - case MODE_6PWM: - *(unsigned *)arg = 6; - break; -#endif - - case MODE_5PWM: - case MODE_5PWM1CAP: - *(unsigned *)arg = 5; - break; - - case MODE_4PWM: - case MODE_4PWM1CAP: - case MODE_4PWM2CAP: - *(unsigned *)arg = 4; - break; - - case MODE_3PWM: - case MODE_3PWM1CAP: - *(unsigned *)arg = 3; - break; - - case MODE_2PWM: - case MODE_2PWM2CAP: - *(unsigned *)arg = 2; - break; - - case MODE_1PWM: - *(unsigned *)arg = 1; - break; - - default: - ret = -EINVAL; - break; - } - - break; - - case PWM_SERVO_SET_MODE: { - switch (arg) { - case PWM_SERVO_MODE_NONE: - ret = set_mode(MODE_NONE); - break; - - case PWM_SERVO_MODE_1PWM: - ret = set_mode(MODE_1PWM); - break; - - case PWM_SERVO_MODE_2PWM: - ret = set_mode(MODE_2PWM); - break; - - case PWM_SERVO_MODE_2PWM2CAP: - ret = set_mode(MODE_2PWM2CAP); - break; - - case PWM_SERVO_MODE_3PWM: - ret = set_mode(MODE_3PWM); - break; + } else { + function -= 1000; + } - case PWM_SERVO_MODE_3PWM1CAP: - ret = set_mode(MODE_3PWM1CAP); - break; + int type = (int)(vehicle_command.param1 + 0.5f); + int index = -1; - case PWM_SERVO_MODE_4PWM: - ret = set_mode(MODE_4PWM); - break; + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + if ((int)_mixing_output.outputFunction(i) == function) { + index = i; + } + } - case PWM_SERVO_MODE_4PWM1CAP: - ret = set_mode(MODE_4PWM1CAP); - break; + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_command.command; + command_ack.target_system = vehicle_command.source_system; + command_ack.target_component = vehicle_command.source_component; + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - case PWM_SERVO_MODE_4PWM2CAP: - ret = set_mode(MODE_4PWM2CAP); - break; + if (index != -1) { + PX4_DEBUG("setting command: index: %i type: %i", index, type); + _current_command.command = dshot_command_t::DShot_cmd_motor_stop; - case PWM_SERVO_MODE_5PWM: - ret = set_mode(MODE_5PWM); - break; + switch (type) { + case 1: _current_command.command = dshot_command_t::DShot_cmd_beacon1; break; - case PWM_SERVO_MODE_5PWM1CAP: - ret = set_mode(MODE_5PWM1CAP); - break; + case 2: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_on; break; - case PWM_SERVO_MODE_6PWM: - ret = set_mode(MODE_6PWM); - break; + case 3: _current_command.command = dshot_command_t::DShot_cmd_3d_mode_off; break; - case PWM_SERVO_MODE_8PWM: - ret = set_mode(MODE_8PWM); - break; + case 4: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_1; break; - case PWM_SERVO_MODE_4CAP: - ret = set_mode(MODE_4CAP); - break; + case 5: _current_command.command = dshot_command_t::DShot_cmd_spin_direction_2; break; + } - case PWM_SERVO_MODE_5CAP: - ret = set_mode(MODE_5CAP); - break; + if (_current_command.command == dshot_command_t::DShot_cmd_motor_stop) { + PX4_WARN("unknown command: %i", type); - case PWM_SERVO_MODE_6CAP: - ret = set_mode(MODE_6CAP); - break; + } else { + command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + _current_command.motor_mask = 1 << index; + _current_command.num_repetitions = 10; + _current_command.save = true; + } - default: - ret = -EINVAL; } - break; + command_ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(command_ack); } + } +} - case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); +void DShot::update_params() +{ + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - break; + updateParams(); - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + // we use a minimum value of 1, since 0 is for disarmed + _mixing_output.setAllMinValues(math::constrain(static_cast((_param_dshot_min.get() * + static_cast(DSHOT_MAX_THROTTLE))), + DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE)); - break; + // Do not use the minimum parameter for reversible outputs + for (unsigned i = 0; i < _num_outputs; ++i) { + if ((1 << i) & _reversible_outputs) { + _mixing_output.minValue(i) = DSHOT_MIN_THROTTLE; } - - default: - ret = -ENOTTY; - break; } - - unlock(); - - return ret; } -int DShot::capture_ioctl(file *filp, const int cmd, const unsigned long arg) +int DShot::ioctl(file *filp, int cmd, unsigned long arg) { - int ret = -EINVAL; - -#if defined(BOARD_HAS_CAPTURE) - - lock(); - - input_capture_config_t *pconfig = 0; - - input_capture_stats_t *stats = (input_capture_stats_t *)arg; + SmartLock lock_guard(_lock); - if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP || - _mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP || - _mode == MODE_4PWM2CAP) { + int ret = OK; - pconfig = (input_capture_config_t *)arg; - } + PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg); switch (cmd) { - - case INPUT_CAP_SET: - if (pconfig) { - ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter, - pconfig->callback, pconfig->context); - } - - break; - - case INPUT_CAP_SET_CALLBACK: - if (pconfig) { - ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context); - } - - break; - - case INPUT_CAP_GET_CALLBACK: - if (pconfig) { - ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context); - } - - break; - - case INPUT_CAP_GET_STATS: - if (arg) { - ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false); - } - - break; - - case INPUT_CAP_GET_CLR_STATS: - if (arg) { - ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true); - } - - break; - - case INPUT_CAP_SET_EDGE: - if (pconfig) { - ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge); - } - - break; - - case INPUT_CAP_GET_EDGE: - if (pconfig) { - ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge); - } - - break; - - case INPUT_CAP_SET_FILTER: - if (pconfig) { - ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter); - } - - break; - - case INPUT_CAP_GET_FILTER: - if (pconfig) { - ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter); - } - - break; - - case INPUT_CAP_GET_COUNT: - ret = OK; - - switch (_mode) { - case MODE_5PWM1CAP: - case MODE_4PWM1CAP: - case MODE_3PWM1CAP: - *(unsigned *)arg = 1; - break; - - case MODE_2PWM2CAP: - case MODE_4PWM2CAP: - *(unsigned *)arg = 2; - break; - - default: - ret = -EINVAL; - break; - } - + case MIXERIOCRESET: + _mixing_output.resetMixer(); break; - case INPUT_CAP_SET_COUNT: - ret = OK; - - switch (_mode) { - case MODE_3PWM1CAP: - set_mode(MODE_3PWM1CAP); - break; - - case MODE_2PWM2CAP: - set_mode(MODE_2PWM2CAP); - break; - - case MODE_4PWM1CAP: - set_mode(MODE_4PWM1CAP); - break; - - case MODE_4PWM2CAP: - set_mode(MODE_4PWM2CAP); - break; - - case MODE_5PWM1CAP: - set_mode(MODE_5PWM1CAP); - break; + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixer(buf, buflen); - default: - ret = -EINVAL; break; } - break; - default: ret = -ENOTTY; break; } - unlock(); - -#else - ret = -ENOTTY; -#endif - return ret; -} - -int DShot::module_new_mode(const PortMode new_mode) -{ - if (!is_running()) { - return -1; - } - - DShot::Mode mode; - - mode = DShot::MODE_NONE; - - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - break; - - case PORT_FULL_PWM: - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4 - // select 4-pin PWM mode - mode = DShot::MODE_4PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5 - mode = DShot::MODE_5PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6 - mode = DShot::MODE_6PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8 - mode = DShot::MODE_8PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 12 - mode = DShot::MODE_12PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14 - mode = DShot::MODE_14PWM; -#endif - break; - - case PORT_PWM1: - // select 2-pin PWM mode - mode = DShot::MODE_1PWM; - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case PORT_PWM8: - // select 8-pin PWM mode - mode = DShot::MODE_8PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case PORT_PWM6: - // select 6-pin PWM mode - mode = DShot::MODE_6PWM; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - case PORT_PWM5: - // select 5-pin PWM mode - mode = DShot::MODE_5PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM5CAP1: - // select 5-pin PWM mode 1 capture - mode = DShot::MODE_5PWM1CAP; - break; - -# endif -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 4 - - case PORT_PWM4: - // select 4-pin PWM mode - mode = DShot::MODE_4PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM4CAP1: - // select 4-pin PWM mode 1 capture - mode = DShot::MODE_4PWM1CAP; - break; - - case PORT_PWM4CAP2: - // select 4-pin PWM mode 2 capture - mode = DShot::MODE_4PWM2CAP; - break; - -# endif - - case PORT_PWM3: - // select 3-pin PWM mode - mode = DShot::MODE_3PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM3CAP1: - // select 3-pin PWM mode 1 capture - mode = DShot::MODE_3PWM1CAP; - break; -# endif - - case PORT_PWM2: - // select 2-pin PWM mode - mode = DShot::MODE_2PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM2CAP2: - // select 2-pin PWM mode 2 capture - mode = DShot::MODE_2PWM2CAP; - break; - -# endif -#endif - - default: - return -1; - } - - DShot *object = get_instance(); - - if (mode != object->get_mode()) { - // (re)set the output mode - object->set_mode(mode); + // if nobody wants it, let CDev have it + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); } - return OK; + return ret; } int DShot::custom_command(int argc, char *argv[]) { - PortMode new_mode = PORT_MODE_UNSET; const char *verb = argv[0]; if (!strcmp(verb, "telemetry")) { @@ -1174,8 +752,8 @@ int DShot::custom_command(int argc, char *argv[]) }; constexpr VerbCommand commands[] = { - {"reverse", DShot_cmd_spin_direction_reversed, 10}, - {"normal", DShot_cmd_spin_direction_normal, 10}, + {"reverse", DShot_cmd_spin_direction_2, 10}, + {"normal", DShot_cmd_spin_direction_1, 10}, {"save", DShot_cmd_save_settings, 10}, {"3d_on", DShot_cmd_3d_mode_on, 10}, {"3d_off", DShot_cmd_3d_mode_off, 10}, @@ -1226,143 +804,13 @@ int DShot::custom_command(int argc, char *argv[]) } } - /* - * Mode switches. - */ - if (!strcmp(verb, "mode_gpio")) { - new_mode = PORT_FULL_GPIO; - - } else if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT_FULL_PWM; - - // mode: defines which outputs to drive (others may be used by other tasks such as camera capture) -#if defined(BOARD_HAS_PWM) - - } else if (!strcmp(verb, "mode_pwm1")) { - new_mode = PORT_PWM1; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - } else if (!strcmp(verb, "mode_pwm6")) { - new_mode = PORT_PWM6; - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - } else if (!strcmp(verb, "mode_pwm5")) { - new_mode = PORT_PWM5; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm5cap1")) { - new_mode = PORT_PWM5CAP1; -# endif - - } else if (!strcmp(verb, "mode_pwm4")) { - new_mode = PORT_PWM4; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm4cap1")) { - new_mode = PORT_PWM4CAP1; - - } else if (!strcmp(verb, "mode_pwm4cap2")) { - new_mode = PORT_PWM4CAP2; -# endif - - } else if (!strcmp(verb, "mode_pwm3")) { - new_mode = PORT_PWM3; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm3cap1")) { - new_mode = PORT_PWM3CAP1; -# endif - - } else if (!strcmp(verb, "mode_pwm2")) { - new_mode = PORT_PWM2; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm2cap2")) { - new_mode = PORT_PWM2CAP2; -# endif -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - } else if (!strcmp(verb, "mode_pwm8")) { - new_mode = PORT_PWM8; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - } else if (!strcmp(verb, "mode_pwm12")) { - new_mode = PORT_PWM12; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - } else if (!strcmp(verb, "mode_pwm14")) { - new_mode = PORT_PWM14; -#endif - } - - // was a new mode set? - if (new_mode != PORT_MODE_UNSET) { - - // switch modes - return DShot::module_new_mode(new_mode); - } - return print_usage("unknown command"); } int DShot::print_status() { - const char *mode_str = nullptr; - - switch (_mode) { - - case MODE_NONE: mode_str = "no outputs"; break; - - case MODE_1PWM: mode_str = "outputs1"; break; - - case MODE_2PWM: mode_str = "outputs2"; break; - - case MODE_2PWM2CAP: mode_str = "outputs2cap2"; break; - - case MODE_3PWM: mode_str = "outputs3"; break; - - case MODE_3PWM1CAP: mode_str = "outputs3cap1"; break; - - case MODE_4PWM: mode_str = "outputs4"; break; - - case MODE_4PWM1CAP: mode_str = "outputs4cap1"; break; - - case MODE_4PWM2CAP: mode_str = "outputs4cap2"; break; - - case MODE_5PWM: mode_str = "outputs5"; break; - - case MODE_5PWM1CAP: mode_str = "outputs5cap1"; break; - - case MODE_6PWM: mode_str = "outputs6"; break; - - case MODE_8PWM: mode_str = "outputs8"; break; - - case MODE_4CAP: mode_str = "cap4"; break; - - case MODE_5CAP: mode_str = "cap5"; break; - - case MODE_6CAP: mode_str = "cap6"; break; - - default: - break; - } - - if (mode_str) { - PX4_INFO("Mode: %s", mode_str); - } - PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no"); + PX4_INFO("Outputs used: 0x%" PRIx32, _output_mask); PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); perf_print_counter(_cycle_perf); _mixing_output.printStatus(); @@ -1387,6 +835,9 @@ int DShot::print_usage(const char *reason) This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement to use DShot as ESC communication protocol instead of PWM. +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 - telemetry via separate UART and publishing as esc_status message @@ -1400,36 +851,7 @@ After saving, the reversed direction will be regarded as the normal one. So to r )DESCR_STR"); PRINT_MODULE_USAGE_NAME("dshot", "driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); - - PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the module if not running already"); - - PRINT_MODULE_USAGE_COMMAND("mode_gpio"); - PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - PRINT_MODULE_USAGE_COMMAND("mode_pwm14"); -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - PRINT_MODULE_USAGE_COMMAND("mode_pwm12"); -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm5"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm2"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2"); -#endif -#if defined(BOARD_HAS_PWM) - PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); -#endif + PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART"); PRINT_MODULE_USAGE_ARG("", "UART device", false); diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 31f009a21c95..1e2169a81059 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -39,13 +39,15 @@ #include #include #include +#include +#include #include "DShotTelemetry.h" using namespace time_literals; -#if !defined(BOARD_HAS_PWM) -# error "board_config.h needs to define BOARD_HAS_PWM" +#if !defined(DIRECT_PWM_OUTPUT_CHANNELS) +# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS" #endif /** Dshot PWM frequency, Hz */ @@ -64,64 +66,13 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn DShot(); virtual ~DShot(); - enum Mode { - MODE_NONE = 0, - MODE_1PWM, - MODE_2PWM, - MODE_2PWM2CAP, - MODE_3PWM, - MODE_3PWM1CAP, - MODE_4PWM, - MODE_4PWM1CAP, - MODE_4PWM2CAP, - MODE_5PWM, - MODE_5PWM1CAP, - MODE_6PWM, - MODE_8PWM, - MODE_12PWM, - MODE_14PWM, - MODE_4CAP, - MODE_5CAP, - MODE_6CAP, - }; - - /** Mode given via CLI */ - enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_PWM, - PORT_PWM14, - PORT_PWM12, - PORT_PWM8, - PORT_PWM6, - PORT_PWM5, - PORT_PWM4, - PORT_PWM3, - PORT_PWM2, - PORT_PWM1, - PORT_PWM3CAP1, - PORT_PWM4CAP1, - PORT_PWM4CAP2, - PORT_PWM5CAP1, - PORT_PWM2CAP2, - PORT_CAPTURE, - }; - - static void capture_trampoline(void *context, const uint32_t channel_index, const hrt_abstime edge_time, - const uint32_t edge_state, const uint32_t overflow); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); - Mode get_mode() { return _mode; } - virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); - /** change the mode of the running module */ - static int module_new_mode(const PortMode new_mode); - void mixerChanged() override; /** @see ModuleBase::print_status() */ @@ -141,8 +92,6 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn */ int send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index); - int set_mode(const Mode new_mode); - /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -169,28 +118,24 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn dshot_command_t command{}; int num_repetitions{0}; uint8_t motor_mask{0xff}; + bool save{false}; + bool valid() const { return num_repetitions > 0; } void clear() { num_repetitions = 0; } }; struct Telemetry { DShotTelemetry handler{}; - uORB::PublicationData esc_status_pub{ORB_ID(esc_status)}; - int last_motor_index{-1}; + uORB::PublicationMultiData esc_status_pub{ORB_ID(esc_status)}; + int last_telemetry_index{-1}; + uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {}; }; - void capture_callback(const uint32_t channel_index, const hrt_abstime edge_time, - const uint32_t edge_state, const uint32_t overflow); - - int capture_ioctl(file *filp, const int cmd, const unsigned long arg); - void enable_dshot_outputs(const bool enabled); void init_telemetry(const char *device); - void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data); - - int pwm_ioctl(file *filp, const int cmd, const unsigned long arg); + void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); int request_esc_info(); @@ -200,7 +145,10 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn void update_telemetry_num_motors(); - MixingOutput _mixing_output{DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + void handle_vehicle_commands(); + + MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + uint32_t _reversible_outputs{}; Telemetry *_telemetry{nullptr}; @@ -215,7 +163,7 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn bool _outputs_on{false}; bool _waiting_for_esc_info{false}; - unsigned _num_outputs{0}; + static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; int _class_instance{-1}; @@ -224,13 +172,16 @@ class DShot : public cdev::CDev, public ModuleBase, public OutputModuleIn Command _current_command{}; - Mode _mode{MODE_NONE}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; DEFINE_PARAMETERS( (ParamInt) _param_dshot_config, (ParamFloat) _param_dshot_min, + (ParamBool) _param_dshot_3d_enable, + (ParamInt) _param_dshot_3d_dead_h, + (ParamInt) _param_dshot_3d_dead_l, (ParamInt) _param_mot_pole_count ) }; diff --git a/src/drivers/dshot/Kconfig b/src/drivers/dshot/Kconfig new file mode 100644 index 000000000000..bed3aaf9d735 --- /dev/null +++ b/src/drivers/dshot/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DSHOT + bool "dshot" + default n + ---help--- + Enable support for dshot \ No newline at end of file diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index a276ceda75bb..850aaf69dfa4 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -14,7 +14,7 @@ parameters: long: | This enables/disables DShot. The different modes define different speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes. - + Note: this enables DShot on the FMU outputs. For boards with an IO it is the AUX outputs. type: enum @@ -40,6 +40,37 @@ parameters: decimal: 2 increment: 0.01 default: 0.055 + DSHOT_3D_ENABLE: + description: + short: Allows for 3d mode when using DShot and suitable mixer + long: | + WARNING: ESC must be configured for 3D mode, and DSHOT_MIN set to 0. + This splits the throttle ranges in two. + Direction 1) 48 is the slowest, 1047 is the fastest. + Direction 2) 1049 is the slowest, 2047 is the fastest. + When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. + type: boolean + default: 0 + DSHOT_3D_DEAD_H: + description: + short: DSHOT 3D deadband high + long: | + When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin. + This value is with respect to the mixer_module range (0-1999), not the DSHOT values. + type: int32 + min: 1000 + max: 1999 + default: 1000 + DSHOT_3D_DEAD_L: + description: + short: DSHOT 3D deadband low + long: | + When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin. + This value is with respect to the mixer_module range (0-1999), not the DSHOT values. + type: int32 + min: 0 + max: 1000 + default: 1000 MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group description: short: Number of magnetic poles of the motors @@ -51,4 +82,3 @@ parameters: Typical motors for 5 inch props have 14 poles. type: int32 default: 14 - diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index dda436354bce..96e517f355ac 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -37,7 +37,6 @@ px4_add_module( MODULE drivers__gps MAIN gps COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable -Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707 SRCS gps.cpp @@ -47,9 +46,10 @@ px4_add_module( devices/src/ubx.cpp devices/src/rtcm.cpp devices/src/emlid_reach.cpp + devices/src/femtomes.cpp + devices/src/nmea.cpp MODULE_CONFIG module.yaml DEPENDS git_gps_devices ) -target_link_libraries(drivers__gps PRIVATE m) diff --git a/src/drivers/gps/Kconfig b/src/drivers/gps/Kconfig new file mode 100644 index 000000000000..334a69fd1725 --- /dev/null +++ b/src/drivers/gps/Kconfig @@ -0,0 +1,12 @@ +menuconfig DRIVERS_GPS + bool "gps" + default n + ---help--- + Enable support for gps + +menuconfig USER_GPS + bool "gps running as userspace module" + default y + depends on BOARD_PROTECTED && DRIVERS_GPS + ---help--- + Put gps in userspace memory diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index fee9bfb6f2cb..5aa4cbb1771a 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -43,6 +43,7 @@ #include #include #include +#include #define GPS_INFO(...) PX4_INFO(__VA_ARGS__) #define GPS_WARN(...) PX4_WARN(__VA_ARGS__) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index f2eb62c2c78a..8c09c5426d23 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit f2eb62c2c78a2ec47bccfe993ff59acc94155e7e +Subproject commit 8c09c5426d23ea4db4e462c1f4e3a1de33d253cc diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5c11a3bd92c2..a4b3615657ef 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -46,7 +46,10 @@ #endif #include +#include +#include +#include #include #include #include @@ -60,11 +63,14 @@ #include #include #include +#include #ifndef CONSTRAINED_FLASH # include "devices/src/ashtech.h" # include "devices/src/emlid_reach.h" # include "devices/src/mtk.h" +# include "devices/src/femtomes.h" +# include "devices/src/nmea.h" #endif // CONSTRAINED_FLASH #include "devices/src/ubx.h" @@ -72,26 +78,35 @@ #include #endif /* __PX4_LINUX */ -#define TIMEOUT_5HZ 500 +#define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error +#define TIMEOUT_5HZ 500 //!< Timeout time in mS, 200 mS (5Hz) + 300 mS delta for error #define RATE_MEASUREMENT_PERIOD 5000000 -typedef enum { - GPS_DRIVER_MODE_NONE = 0, - GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_ASHTECH, - GPS_DRIVER_MODE_EMLIDREACH -} gps_driver_mode_t; +enum class gps_driver_mode_t { + None = 0, + UBX, + MTK, + ASHTECH, + EMLIDREACH, + FEMTOMES, + NMEA +}; + +enum class gps_dump_comm_mode_t : int32_t { + Disabled = 0, + Full, ///< dump full RX and TX data for all devices + RTCM ///< dump received RTCM from Main GPS +}; /* struct for dynamic allocation of satellite info data */ struct GPS_Sat_Info { satellite_info_s _data; }; -static constexpr int TASK_STACK_SIZE = 1760; +static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1760); -class GPS : public ModuleBase +class GPS : public ModuleBase, public device::Device { public: @@ -103,7 +118,7 @@ class GPS : public ModuleBase Count }; - GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, Instance instance, + GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance, unsigned configured_baudrate); ~GPS() override; @@ -167,6 +182,8 @@ class GPS : public ModuleBase satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position + uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; + uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info float _rate{0.0f}; ///< position update rate @@ -177,11 +194,12 @@ class GPS : public ModuleBase const Instance _instance; - uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; - uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; - gps_dump_s *_dump_to_device{nullptr}; - gps_dump_s *_dump_from_device{nullptr}; - bool _should_dump_communication{false}; ///< if true, dump communication + uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; + gps_dump_s *_dump_to_device{nullptr}; + gps_dump_s *_dump_from_device{nullptr}; + gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -200,6 +218,16 @@ class GPS : public ModuleBase */ void publishSatelliteInfo(); + /** + * Publish RTCM corrections + */ + void publishRTCMCorrections(uint8_t *data, size_t len); + + /** + * Publish RTCM corrections + */ + void publishRelativePosition(sensor_gnss_relative_s &gnss_relative); + /** * This is an abstraction for the poll on serial used. * @@ -240,9 +268,10 @@ class GPS : public ModuleBase * Dump gps communication. * @param data message * @param len length of the message + * @param mode calling source * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device */ - void dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device); + void dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device); void initializeCommunicationDump(); @@ -258,8 +287,9 @@ px4::atomic GPS::_secondary_instance{nullptr}; extern "C" __EXPORT int gps_main(int argc, char *argv[]); -GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, - Instance instance, unsigned configured_baudrate) : +GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, Instance instance, + unsigned configured_baudrate) : + Device(MODULE_NAME), _configured_baudrate(configured_baudrate), _mode(mode), _interface(interface), @@ -273,6 +303,9 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = NAN; + int32_t enable_sat_info = 0; + param_get(param_find("GPS_SAT_INFO"), &enable_sat_info); + /* create satellite info data object if requested */ if (enable_sat_info) { _sat_info = new GPS_Sat_Info(); @@ -280,7 +313,17 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info)); } - if (_mode == GPS_DRIVER_MODE_NONE) { + if (_interface == GPSHelper::Interface::UART) { + set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL); + + char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) + set_device_bus(atoi(&c)); + + } else if (_interface == GPSHelper::Interface::SPI) { + set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI); + } + + if (_mode == gps_driver_mode_t::None) { // use parameter to select mode if not provided via CLI char protocol_param_name[16]; snprintf(protocol_param_name, sizeof(protocol_param_name), "GPS_%i_PROTOCOL", (int)_instance + 1); @@ -288,19 +331,23 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac param_get(param_find(protocol_param_name), &protocol); switch (protocol) { - case 1: _mode = GPS_DRIVER_MODE_UBX; break; + case 1: _mode = gps_driver_mode_t::UBX; break; #ifndef CONSTRAINED_FLASH - case 2: _mode = GPS_DRIVER_MODE_MTK; break; + case 2: _mode = gps_driver_mode_t::MTK; break; + + case 3: _mode = gps_driver_mode_t::ASHTECH; break; + + case 4: _mode = gps_driver_mode_t::EMLIDREACH; break; - case 3: _mode = GPS_DRIVER_MODE_ASHTECH; break; + case 5: _mode = gps_driver_mode_t::FEMTOMES; break; - case 4: _mode = GPS_DRIVER_MODE_EMLIDREACH; break; + case 6: _mode = gps_driver_mode_t::NMEA; break; #endif // CONSTRAINED_FLASH } } - _mode_auto = _mode == GPS_DRIVER_MODE_NONE; + _mode_auto = _mode == gps_driver_mode_t::None; } GPS::~GPS() @@ -333,25 +380,35 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) switch (type) { case GPSCallbackType::readDeviceData: { - int num_read = gps->pollOrRead((uint8_t *)data1, data2, *((int *)data1)); + int timeout; + memcpy(&timeout, data1, sizeof(timeout)); + int num_read = gps->pollOrRead((uint8_t *)data1, data2, timeout); if (num_read > 0) { - gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, false); + gps->dumpGpsData((uint8_t *)data1, (size_t)num_read, gps_dump_comm_mode_t::Full, false); } return num_read; } case GPSCallbackType::writeDeviceData: - gps->dumpGpsData((uint8_t *)data1, (size_t)data2, true); + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); - return write(gps->_serial_fd, data1, (size_t)data2); + return ::write(gps->_serial_fd, data1, (size_t)data2); case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); case GPSCallbackType::gotRTCMMessage: - /* not used */ + gps->publishRTCMCorrections((uint8_t *)data1, (size_t)data2); + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::RTCM, false); + break; + + case GPSCallbackType::gotRelativePositionMessage: + if (data1 && data2 == sizeof(sensor_gnss_relative_s)) { + gps->publishRelativePosition(*static_cast(data1)); + } + break; case GPSCallbackType::surveyInStatus: @@ -417,7 +474,7 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) #ifdef __PX4_NUTTX int err = 0; int bytes_available = 0; - err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); if (err != 0 || bytes_available < (int)character_count) { px4_usleep(sleeptime); @@ -472,13 +529,16 @@ void GPS::handleInjectDataTopic() if (_orb_inject_data_sub.copy(&msg)) { - /* Write the message to the gps device. Note that the message could be fragmented. - * But as we don't write anywhere else to the device during operation, we don't - * need to assemble the message first. - */ - injectData(msg.data, msg.len); + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { + /* Write the message to the gps device. Note that the message could be fragmented. + * But as we don't write anywhere else to the device during operation, we don't + * need to assemble the message first. + */ + injectData(msg.data, msg.len); - ++_last_rate_rtcm_injection_count; + ++_last_rate_rtcm_injection_count; + } } } } while (updated && num_injections < max_num_injections); @@ -486,7 +546,7 @@ void GPS::handleInjectDataTopic() bool GPS::injectData(uint8_t *data, size_t len) { - dumpGpsData(data, len, true); + dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); size_t written = ::write(_serial_fd, data, len); ::fsync(_serial_fd); @@ -517,6 +577,12 @@ int GPS::setBaudrate(unsigned baud) case 460800: speed = B460800; break; +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + default: PX4_ERR("ERR: unknown baudrate: %d", baud); return -EINVAL; @@ -592,7 +658,7 @@ void GPS::initializeCommunicationDump() return; } - if (param_dump_comm != 1) { + if (param_dump_comm < 1 || param_dump_comm > 2) { return; //dumping disabled } @@ -609,19 +675,20 @@ void GPS::initializeCommunicationDump() //make sure to use a large enough queue size, so that we don't lose messages. You may also want //to increase the logger rate for that. - _dump_communication_pub.publish(*_dump_from_device); + _dump_communication_pub.advertise(); - _should_dump_communication = true; + _dump_communication_mode = (gps_dump_comm_mode_t)param_dump_comm; } -void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) +void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device) { - if (!_should_dump_communication) { + gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; + + if (_dump_communication_mode != mode || !dump_data) { return; } - gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; - dump_data->instance = (uint8_t) _instance; + dump_data->instance = (uint8_t)_instance; while (len > 0) { size_t write_len = len; @@ -650,35 +717,6 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) void GPS::run() { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); - - if (_serial_fd < 0) { - PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); - return; - } - -#ifdef __PX4_LINUX - - if (_interface == GPSHelper::Interface::SPI) { - int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) - int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - return; - } - - status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); - - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - return; - } - } - -#endif /* __PX4_LINUX */ - param_t handle = param_find("GPS_YAW_OFFSET"); float heading_offset = 0.f; @@ -709,6 +747,20 @@ GPS::run() } else { ubx_mode = GPSDriverUBX::UBXMode::MovingBase; } + + } else if (gps_ubx_mode == 2) { + ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + + } else if (gps_ubx_mode == 3) { + if (_instance == Instance::Main) { + ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1; + + } else { + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; + } + + } else if (gps_ubx_mode == 4) { + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; } } @@ -735,27 +787,71 @@ GPS::run() _helper = nullptr; } + if (_serial_fd < 0) { + /* open the serial port */ + _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + if (_serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + px4_sleep(1); + continue; + } + +#ifdef __PX4_LINUX + + if (_interface == GPSHelper::Interface::SPI) { + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + } + + status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + + if (status_value < 0) { + PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + } + } + +#endif /* __PX4_LINUX */ + } + switch (_mode) { - case GPS_DRIVER_MODE_NONE: - _mode = GPS_DRIVER_MODE_UBX; + case gps_driver_mode_t::None: + _mode = gps_driver_mode_t::UBX; /* FALLTHROUGH */ - case GPS_DRIVER_MODE_UBX: + case gps_driver_mode_t::UBX: _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info, gps_ubx_dynmodel, heading_offset, ubx_mode); + set_device_type(DRV_GPS_DEVTYPE_UBX); break; #ifndef CONSTRAINED_FLASH - case GPS_DRIVER_MODE_MTK: + case gps_driver_mode_t::MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); + set_device_type(DRV_GPS_DEVTYPE_MTK); break; - case GPS_DRIVER_MODE_ASHTECH: + case gps_driver_mode_t::ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); + set_device_type(DRV_GPS_DEVTYPE_ASHTECH); break; - case GPS_DRIVER_MODE_EMLIDREACH: + case gps_driver_mode_t::EMLIDREACH: _helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); + set_device_type(DRV_GPS_DEVTYPE_EMLID_REACH); + break; + + case gps_driver_mode_t::FEMTOMES: + _helper = new GPSDriverFemto(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); + set_device_type(DRV_GPS_DEVTYPE_FEMTOMES); + break; + + case gps_driver_mode_t::NMEA: + _helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); + set_device_type(DRV_GPS_DEVTYPE_NMEA); break; #endif // CONSTRAINED_FLASH @@ -765,9 +861,15 @@ GPS::run() _baudrate = _configured_baudrate; GPSHelper::GPSConfig gpsConfig{}; - gpsConfig.output_mode = GPSHelper::OutputMode::GPS; gpsConfig.gnss_systems = static_cast(gnssSystemsParam); + if (_instance == Instance::Main && _dump_communication_mode == gps_dump_comm_mode_t::RTCM) { + gpsConfig.output_mode = GPSHelper::OutputMode::GPSAndRTCM; + + } else { + gpsConfig.output_mode = GPSHelper::OutputMode::GPS; + } + if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) { /* reset report */ @@ -775,15 +877,54 @@ GPS::run() _report_gps_pos.heading = NAN; _report_gps_pos.heading_offset = heading_offset; - if (_mode == GPS_DRIVER_MODE_UBX) { + if (_mode == gps_driver_mode_t::UBX) { /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); + + // populate specific ublox model + if (get_device_type() == DRV_GPS_DEVTYPE_UBX) { + GPSDriverUBX *driver_ubx = (GPSDriverUBX *)_helper; + + switch (driver_ubx->board()) { + case GPSDriverUBX::Board::u_blox6: + set_device_type(DRV_GPS_DEVTYPE_UBX_6); + break; + + case GPSDriverUBX::Board::u_blox7: + set_device_type(DRV_GPS_DEVTYPE_UBX_7); + break; + + case GPSDriverUBX::Board::u_blox8: + set_device_type(DRV_GPS_DEVTYPE_UBX_8); + break; + + case GPSDriverUBX::Board::u_blox9: + set_device_type(DRV_GPS_DEVTYPE_UBX_9); + break; + + case GPSDriverUBX::Board::u_blox9_F9P: + set_device_type(DRV_GPS_DEVTYPE_UBX_F9P); + break; + + default: + set_device_type(DRV_GPS_DEVTYPE_UBX); + break; + } + } } int helper_ret; + unsigned receive_timeout = TIMEOUT_5HZ; + + if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) + || (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1)) { + /* The MB rover will wait as long as possible to compute a navigation solution, + * possibly lowering the navigation rate all the way to 1 Hz while doing so. */ + receive_timeout = TIMEOUT_1HZ; + } - while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) { + while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) { if (helper_ret & 1) { publish(); @@ -816,19 +957,19 @@ GPS::run() // const char *mode_str = "unknown"; // // switch (_mode) { -// case GPS_DRIVER_MODE_UBX: +// case gps_driver_mode_t::UBX: // mode_str = "UBX"; // break; // -// case GPS_DRIVER_MODE_MTK: +// case gps_driver_mode_t::MTK: // mode_str = "MTK"; // break; // -// case GPS_DRIVER_MODE_ASHTECH: +// case gps_driver_mode_t::ASHTECH: // mode_str = "ASHTECH"; // break; // -// case GPS_DRIVER_MODE_EMLIDREACH: +// case gps_driver_mode_t::EMLIDREACH: // mode_str = "EMLID REACH"; // break; // @@ -848,24 +989,34 @@ GPS::run() } } + if (_serial_fd >= 0) { + ::close(_serial_fd); + _serial_fd = -1; + } + if (_mode_auto) { switch (_mode) { - case GPS_DRIVER_MODE_UBX: + case gps_driver_mode_t::UBX: #ifndef CONSTRAINED_FLASH - _mode = GPS_DRIVER_MODE_MTK; + _mode = gps_driver_mode_t::MTK; + break; + + case gps_driver_mode_t::MTK: + _mode = gps_driver_mode_t::ASHTECH; break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; + case gps_driver_mode_t::ASHTECH: + _mode = gps_driver_mode_t::EMLIDREACH; break; - case GPS_DRIVER_MODE_ASHTECH: - _mode = GPS_DRIVER_MODE_EMLIDREACH; + case gps_driver_mode_t::EMLIDREACH: + _mode = gps_driver_mode_t::FEMTOMES; break; - case GPS_DRIVER_MODE_EMLIDREACH: + case gps_driver_mode_t::FEMTOMES: + case gps_driver_mode_t::NMEA: // skip NMEA for auto-detection to avoid false positive matching #endif // CONSTRAINED_FLASH - _mode = GPS_DRIVER_MODE_UBX; + _mode = gps_driver_mode_t::UBX; px4_usleep(500000); // tried all possible drivers. Wait a bit before next round break; @@ -879,11 +1030,6 @@ GPS::run() } PX4_INFO("exiting"); - - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; - } } int @@ -905,22 +1051,30 @@ GPS::print_status() // GPS Mode switch (_mode) { - case GPS_DRIVER_MODE_UBX: + case gps_driver_mode_t::UBX: PX4_INFO("protocol: UBX"); break; #ifndef CONSTRAINED_FLASH - case GPS_DRIVER_MODE_MTK: + case gps_driver_mode_t::MTK: PX4_INFO("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: + case gps_driver_mode_t::ASHTECH: PX4_INFO("protocol: ASHTECH"); break; - case GPS_DRIVER_MODE_EMLIDREACH: + case gps_driver_mode_t::EMLIDREACH: PX4_INFO("protocol: EMLIDREACH"); break; + + case gps_driver_mode_t::FEMTOMES: + PX4_INFO("protocol: FEMTOMES"); + break; + + case gps_driver_mode_t::NMEA: + PX4_INFO("protocol: NMEA"); + break; #endif // CONSTRAINED_FLASH default: @@ -940,7 +1094,7 @@ GPS::print_status() PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate); PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); - print_message(_report_gps_pos); + print_message(ORB_ID(sensor_gps), _report_gps_pos); } if (_instance == Instance::Main && _secondary_instance.load()) { @@ -987,6 +1141,8 @@ void GPS::publish() { if (_instance == Instance::Main || _is_gps_main_advertised.load()) { + _report_gps_pos.device_id = get_device_id(); + _report_gps_pos_pub.publish(_report_gps_pos); // Heading/yaw data can be updated at a lower rate than the other navigation data. // The uORB message definition requires this data to be set to a NAN if no new valid data is available. @@ -1008,6 +1164,49 @@ GPS::publishSatelliteInfo() } } +void +GPS::publishRTCMCorrections(uint8_t *data, size_t len) +{ + gps_inject_data_s gps_inject_data{}; + + gps_inject_data.timestamp = hrt_absolute_time(); + gps_inject_data.device_id = get_device_id(); + + size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); + + if (len > capacity) { + gps_inject_data.flags = 1; //LSB: 1=fragmented + + } else { + gps_inject_data.flags = 0; + } + + size_t written = 0; + + while (written < len) { + + gps_inject_data.len = len - written; + + if (gps_inject_data.len > capacity) { + gps_inject_data.len = capacity; + } + + memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); + + _gps_inject_data_pub.publish(gps_inject_data); + + written = written + gps_inject_data.len; + } +} + +void +GPS::publishRelativePosition(sensor_gnss_relative_s &gnss_relative) +{ + gnss_relative.device_id = get_device_id(); + gnss_relative.timestamp = hrt_absolute_time(); + _sensor_gnss_relative_pub.publish(gnss_relative); +} + int GPS::custom_command(int argc, char *argv[]) { @@ -1081,11 +1280,9 @@ Initiate warm restart of GPS device PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Optional secondary GPS device", true); PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:)", true); - PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true); - PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true); PRINT_MODULE_USAGE_PARAM_STRING('j', "uart", "spi|uart", "secondary GPS interface", true); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml", "GPS Protocol (default=auto select)", true); + PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "ubx|mtk|ash|eml|fem|nmea", "GPS Protocol (default=auto select)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset GPS device"); @@ -1147,21 +1344,20 @@ GPS *GPS::instantiate(int argc, char *argv[]) GPS *GPS::instantiate(int argc, char *argv[], Instance instance) { - const char *device_name = "/dev/ttyS3"; + const char *device_name = nullptr; const char *device_name_secondary = nullptr; int baudrate_main = 0; int baudrate_secondary = 0; - bool enable_sat_info = false; GPSHelper::Interface interface = GPSHelper::Interface::UART; GPSHelper::Interface interface_secondary = GPSHelper::Interface::UART; - gps_driver_mode_t mode = GPS_DRIVER_MODE_NONE; + gps_driver_mode_t mode = gps_driver_mode_t::None; bool error_flag = false; int myoptind = 1; int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "b:d:e:g:si:j:p:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:d:e:g:i:j:p:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) { @@ -1184,10 +1380,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) device_name_secondary = myoptarg; break; - case 's': - enable_sat_info = true; - break; - case 'i': if (!strcmp(myoptarg, "spi")) { interface = GPSHelper::Interface::SPI; @@ -1216,16 +1408,22 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) case 'p': if (!strcmp(myoptarg, "ubx")) { - mode = GPS_DRIVER_MODE_UBX; + mode = gps_driver_mode_t::UBX; #ifndef CONSTRAINED_FLASH } else if (!strcmp(myoptarg, "mtk")) { - mode = GPS_DRIVER_MODE_MTK; + mode = gps_driver_mode_t::MTK; } else if (!strcmp(myoptarg, "ash")) { - mode = GPS_DRIVER_MODE_ASHTECH; + mode = gps_driver_mode_t::ASHTECH; } else if (!strcmp(myoptarg, "eml")) { - mode = GPS_DRIVER_MODE_EMLIDREACH; + mode = gps_driver_mode_t::EMLIDREACH; + + } else if (!strcmp(myoptarg, "fem")) { + mode = gps_driver_mode_t::FEMTOMES; + + } else if (!strcmp(myoptarg, "nmea")) { + mode = gps_driver_mode_t::NMEA; #endif // CONSTRAINED_FLASH } else { PX4_ERR("unknown protocol: %s", myoptarg); @@ -1248,9 +1446,14 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) return nullptr; } - GPS *gps; + GPS *gps = nullptr; if (instance == Instance::Main) { - gps = new GPS(device_name, mode, interface, enable_sat_info, instance, baudrate_main); + if (device_name && (access(device_name, R_OK|W_OK) == 0)) { + gps = new GPS(device_name, mode, interface, instance, baudrate_main); + + } else { + PX4_ERR("invalid device (-d) %s", device_name ? device_name : ""); + } if (gps && device_name_secondary) { task_spawn(argc, argv, Instance::Secondary); @@ -1268,7 +1471,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } } } else { // secondary instance - gps = new GPS(device_name_secondary, mode, interface_secondary, enable_sat_info, instance, baudrate_secondary); + if (device_name_secondary && (access(device_name_secondary, R_OK|W_OK) == 0)) { + gps = new GPS(device_name_secondary, mode, interface_secondary, instance, baudrate_secondary); + + } else { + PX4_ERR("invalid secondary device (-g) %s", device_name_secondary ? device_name_secondary : ""); + } } return gps; diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index fd64bf221d1d..25a0cb293461 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -32,14 +32,19 @@ ****************************************************************************/ /** - * Dump GPS communication to a file. + * Log GPS communication data * * If this is set to 1, all GPS communication data will be published via uORB, * and written to the log file as gps_dump message. + * + * If this is set to 2, the main GPS is configured to output RTCM data, + * which is then logged as gps_dump and can be used for PPK. + * * @min 0 - * @max 1 + * @max 2 * @value 0 Disable - * @value 1 Enable + * @value 1 Full communication + * @value 2 RTCM output (PPK) * @group GPS */ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0); @@ -64,6 +69,18 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0); */ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); +/** + * Enable sat info (if available) + * + * Enable publication of satellite info (ORB_ID(satellite_info)) if possible. + * Not available on MTK. + * + * @boolean + * @reboot_required true + * @group GPS + */ +PARAM_DEFINE_INT32(GPS_SAT_INFO, 0); + /** * u-blox GPS Mode * @@ -71,14 +88,19 @@ PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); * dual GPS without heading. * * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output - * heading information, whereas the secondary will act as moving base, sending RTCM on UART2 to - * the rover GPS. + * heading information, whereas the secondary will act as moving base. + * Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the + * F9P units are connected to each other. + * Modes 3 and 4 only require UART1 on each F9P connected to the Autopilot or Can Node. UART RX DMA is required. * RTK is still possible with this setup. * * @min 0 * @max 1 * @value 0 Default - * @value 1 Heading + * @value 1 Heading (Rover With Moving Base UART1 Connected To Autopilot, UART2 Connected To Moving Base) + * @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover) + * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600) + * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600) * * @reboot_required true * @group GPS @@ -90,12 +112,11 @@ PARAM_DEFINE_INT32(GPS_UBX_MODE, 0); * Heading/Yaw offset for dual antenna GPS * * Heading offset angle for dual antenna GPS setups that support heading estimation. - * (currently only for the Trimble MB-Two). * - * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in + * Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the rover antenna is in * front. The offset angle increases clockwise. * - * Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle. + * Set this to 90 if the rover antenna is placed on the right side of the vehicle and the moving base antenna is on the left side. * * @min 0 * @max 360 @@ -115,12 +136,14 @@ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f); * Auto-detection will probe all protocols, and thus is a bit slower. * * @min 0 - * @max 4 + * @max 5 * @value 0 Auto detect * @value 1 u-blox * @value 2 MTK * @value 3 Ashtech / Trimble * @value 4 Emlid Reach + * @value 5 Femtomes + * @value 6 NMEA (generic) * * @reboot_required true * @group GPS @@ -135,12 +158,14 @@ PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1); * Auto-detection will probe all protocols, and thus is a bit slower. * * @min 0 - * @max 4 + * @max 5 * @value 0 Auto detect * @value 1 u-blox * @value 2 MTK * @value 3 Ashtech / Trimble * @value 4 Emlid Reach + * @value 5 Femtomes + * @value 6 NMEA (generic) * * @reboot_required true * @group GPS diff --git a/src/drivers/heater/Kconfig b/src/drivers/heater/Kconfig new file mode 100644 index 000000000000..a90575ed090b --- /dev/null +++ b/src/drivers/heater/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_HEATER + bool "heater" + default n + ---help--- + Enable support for heater \ No newline at end of file diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index aca5d55eca66..170bd859a49f 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -48,7 +48,7 @@ #include #include -#if defined(BOARD_USES_PX4IO) and defined(PX4IO_HEATER_ENABLED) +#if defined(BOARD_USES_PX4IO_VERSION) and defined(PX4IO_HEATER_ENABLED) // Heater on some boards is on IO MCU // Use ioctl calls to IO driver to turn heater on/off # define HEATER_PX4IO @@ -73,11 +73,13 @@ Heater::Heater() : } #endif + + _heater_status_pub.advertise(); } Heater::~Heater() { - heater_disable(); + disable_heater(); } int Heater::custom_command(int argc, char *argv[]) @@ -91,7 +93,7 @@ int Heater::custom_command(int argc, char *argv[]) return print_usage("Unrecognized command."); } -void Heater::heater_disable() +void Heater::disable_heater() { // Reset heater to off state. #ifdef HEATER_PX4IO @@ -106,7 +108,7 @@ void Heater::heater_disable() #endif } -void Heater::heater_initialize() +void Heater::initialize_heater_io() { // Initialize heater to off state. #ifdef HEATER_PX4IO @@ -136,7 +138,7 @@ void Heater::heater_off() #endif #ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); + HEATER_OUTPUT_EN(false); #endif } @@ -151,7 +153,7 @@ void Heater::heater_on() #endif #ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); + HEATER_OUTPUT_EN(true); #endif } @@ -160,13 +162,15 @@ bool Heater::initialize_topics() for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0 - && PX4_ISFINITE(sensor_accel_sub.get().temperature)) { + if (sensor_accel_sub.get().timestamp != 0 && + sensor_accel_sub.get().device_id != 0 && + PX4_ISFINITE(sensor_accel_sub.get().temperature)) { // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) { _sensor_accel_sub.ChangeInstance(i); _sensor_device_id = sensor_accel_sub.get().device_id; + initialize_heater_io(); return true; } } @@ -190,21 +194,10 @@ void Heater::Run() return; } - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - ModuleParams::updateParams(); - } + update_params(); if (_sensor_device_id == 0) { - if (initialize_topics()) { - heater_initialize(); - - } else { + if (!initialize_topics()) { // if sensor still not found try again in 1 second ScheduleDelayed(1_s); return; @@ -212,14 +205,15 @@ void Heater::Run() } sensor_accel_s sensor_accel; + float temperature_delta {0.f}; if (_heater_on) { // Turn the heater off. - heater_off(); _heater_on = false; + heater_off(); + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); } else if (_sensor_accel_sub.update(&sensor_accel)) { - float temperature_delta {0.f}; // Update the current IMU sensor temperature if valid. if (PX4_ISFINITE(sensor_accel.temperature)) { @@ -230,11 +224,6 @@ void Heater::Run() _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); - if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { - _integrator_value = 0.f; - } - - // Guard against integrator wind up. _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + @@ -242,36 +231,41 @@ void Heater::Run() _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); + if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { + _temperature_target_met = true; + + } else { + + _temperature_target_met = false; + } + _heater_on = true; heater_on(); - } - - // Schedule the next cycle. - if (_heater_on) { ScheduleDelayed(_controller_time_on_usec); - - } else { - ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); } + publish_status(); +} - // publish status +void Heater::publish_status() +{ heater_status_s status{}; - status.heater_on = _heater_on; - status.device_id = _sensor_device_id; - status.temperature_sensor = _temperature_last; - status.temperature_target = _param_sens_imu_temp.get(); - status.controller_period_usec = _controller_period_usec; + status.device_id = _sensor_device_id; + status.heater_on = _heater_on; + status.temperature_sensor = _temperature_last; + status.temperature_target = _param_sens_imu_temp.get(); + status.temperature_target_met = _temperature_target_met; + status.controller_period_usec = _controller_period_usec; status.controller_time_on_usec = _controller_time_on_usec; - status.proportional_value = _proportional_value; - status.integrator_value = _integrator_value; - status.feed_forward_value = _param_sens_imu_temp_ff.get(); + status.proportional_value = _proportional_value; + status.integrator_value = _integrator_value; + status.feed_forward_value = _param_sens_imu_temp_ff.get(); #ifdef HEATER_PX4IO - status.mode |= heater_status_s::MODE_PX4IO; + status.mode = heater_status_s::MODE_PX4IO; #endif #ifdef HEATER_GPIO - status.mode |= heater_status_s::MODE_GPIO; + status.mode = heater_status_s::MODE_GPIO; #endif status.timestamp = hrt_absolute_time(); @@ -287,6 +281,7 @@ int Heater::start() return PX4_ERROR; } + update_params(true); ScheduleNow(); return PX4_OK; } @@ -307,6 +302,18 @@ int Heater::task_spawn(int argc, char *argv[]) return 0; } +void Heater::update_params(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // update parameters from storage + ModuleParams::updateParams(); + } +} + int Heater::print_usage(const char *reason) { if (reason) { diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 37be61063058..966b04019c0a 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -57,7 +57,8 @@ using namespace time_literals; -#define CONTROLLER_PERIOD_DEFAULT 100000 +#define CONTROLLER_PERIOD_DEFAULT 10000 +#define TEMPERATURE_TARGET_THRESHOLD 2.5f class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -101,9 +102,25 @@ class Heater : public ModuleBase, public ModuleParams, public px4::Sched private: + /** Disables the heater (either by GPIO or PX4IO). */ + void disable_heater(); + + /** Turns the heater on (either by GPIO or PX4IO). */ + void heater_on(); + + /** Turns the heater off (either by GPIO or PX4IO). */ + void heater_off(); + + void initialize(); + + /** Enables / configures the heater (either by GPIO or PX4IO). */ + void initialize_heater_io(); + /** @brief Called once to initialize uORB topics. */ bool initialize_topics(); + void publish_status(); + /** @brief Calculates the heater element on/off time and schedules the next cycle. */ void Run() override; @@ -113,27 +130,17 @@ class Heater : public ModuleBase, public ModuleParams, public px4::Sched */ void update_params(const bool force = false); - /** Enables / configures the heater (either by GPIO or PX4IO). */ - void heater_initialize(); - - /** Disnables the heater (either by GPIO or PX4IO). */ - void heater_disable(); - - /** Turns the heater on (either by GPIO or PX4IO). */ - void heater_on(); - - /** Turns the heater off (either by GPIO or PX4IO). */ - void heater_off(); - /** Work queue struct for the scheduler. */ static struct work_s _work; /** File descriptor for PX4IO for heater ioctl's */ -#if defined(HEATER_PX4IO) - int _io_fd_ {-1}; +#if defined(PX4IO_HEATER_ENABLED) + int _io_fd {-1}; #endif - bool _heater_on = false; + bool _heater_initialized = false; + bool _heater_on = false; + bool _temperature_target_met = false; int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; int _controller_time_on_usec = 0; @@ -155,7 +162,7 @@ class Heater : public ModuleBase, public ModuleParams, public px4::Sched (ParamFloat) _param_sens_imu_temp_ff, (ParamFloat) _param_sens_imu_temp_i, (ParamFloat) _param_sens_imu_temp_p, - (ParamInt) _param_sens_temp_id, - (ParamFloat) _param_sens_imu_temp + (ParamFloat) _param_sens_imu_temp, + (ParamInt) _param_sens_temp_id ) }; diff --git a/src/drivers/hygrometer/Kconfig b/src/drivers/hygrometer/Kconfig new file mode 100644 index 000000000000..caf5432178eb --- /dev/null +++ b/src/drivers/hygrometer/Kconfig @@ -0,0 +1,9 @@ +menu "Hygrometers" + menuconfig COMMON_HYGROMETERS + bool "Hygrometer sensors" + default n + select DRIVERS_HYGROMETER_SHT3x + ---help--- + Enable default set of hygrometer sensors + rsource "*/Kconfig" +endmenu #Hygrometers diff --git a/src/drivers/hygrometer/sht3x/CMakeLists.txt b/src/drivers/hygrometer/sht3x/CMakeLists.txt new file mode 100644 index 000000000000..2edf899151a9 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__hygrometer__sht3x + MAIN sht3x + COMPILE_FLAGS + SRCS + sht3x.cpp + sht3x.h + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/hygrometer/sht3x/Kconfig b/src/drivers/hygrometer/sht3x/Kconfig new file mode 100644 index 000000000000..424ec7948193 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_HYGROMETER_SHT3x + bool "SHT3x temperature and humidity sensor" + default n + ---help--- + Enable support SHT35 temperature/humidity sensor diff --git a/src/drivers/hygrometer/sht3x/sht3x.cpp b/src/drivers/hygrometer/sht3x/sht3x.cpp new file mode 100644 index 000000000000..ff68cd62c3d9 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x.cpp @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * 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 sht3x.c + * + * I2C driver for temperature/humidity sensor SHT3x by senserion + * + * @author Roman Dvorak + * + */ + +#include "sht3x.h" + +SHT3X::SHT3X(const I2CSPIDriverConfig &config) : + I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config) +{ +} + +uint8_t SHT3X::calc_crc(uint8_t data[2]) +{ + uint8_t crc = 0xFF; + + for (int i = 0; i < 2; i++) { + crc ^= data[i]; + + for (uint8_t bit = 8; bit > 0; --bit) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0x31u; + + } else { + crc = (crc << 1); + } + } + } + + return crc; +} + + +int SHT3X::set_pointer(uint16_t command) +{ + if (_last_command != command) { + uint8_t cmd[2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + _last_command = command; + return transfer(&cmd[0], 2, nullptr, 0); + + } else { + return 0; + } +} + +int SHT3X::read_data(uint16_t command, uint8_t *data_ptr, uint8_t length) +{ + set_pointer(command); + + uint8_t raw_data[length]; + transfer(nullptr, 0, &raw_data[0], length); + + uint8_t crc_err = 0; + + for (int i = 0; i < length / 3; ++i) { + uint8_t crc_data[2] = {raw_data[i * 3], raw_data[i * 3 + 1]}; + + if (raw_data[i * 3 + 2] != calc_crc(crc_data)) { + crc_err ++; + } + + *(data_ptr + i * 2) = raw_data[i * 3]; + *(data_ptr + i * 2 + 1) = raw_data[i * 3 + 1]; + } + + return crc_err; +} + +int SHT3X::write_data(uint16_t command, uint8_t buffer[], uint8_t length) +{ + _last_command = command; + + uint8_t cmd[2 + 3 * length / 2]; + cmd[0] = static_cast(command >> 8); + cmd[1] = static_cast(command & 0xff); + + for (int i = 0; i < length / 2; ++i) { + cmd[2 + 3 * i] = buffer[2 * i]; + cmd[2 + 3 * i + 1] = buffer[2 * i + 1]; + uint8_t crc_data[2] = {buffer[2 * i], buffer[2 * i + 1]}; + cmd[2 + 3 * i + 2] = calc_crc(crc_data); + } + + return transfer(&cmd[0], sizeof(cmd), nullptr, 0); +} + + +void SHT3X::sensor_compouse_msg(bool send) +{ + uint8_t data[4]; + int error = read_data(SHT3x_CMD_FETCH_DATA, &data[0], 6); + + if (error == PX4_OK) { + measurement_time = hrt_absolute_time(); + measurement_index ++; + + measured_temperature = (float) 175 * (data[0] << 8 | data[1]) / 65535 - 45; + measured_humidity = (float) 100 * (data[2] << 8 | data[3]) / 65535; + + if (send) { + sensor_hygrometer_s msg{}; + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = measurement_time; + msg.temperature = measured_temperature; + msg.humidity = measured_humidity; + msg.device_id = _sht_info.serial_number; + _sensor_hygrometer_pub.publish(msg); + } + } +} + + +int +SHT3X::probe() +{ + uint8_t type[2]; + uint8_t nvalid; + + nvalid = read_data(SHT3X_CMD_READ_STATUS, &type[0], 3); + return (!(nvalid == PX4_OK)); + // 0 means I can see sensor +} + +int SHT3X::init() +{ + if (I2C::init() != PX4_OK) { + return PX4_ERROR; + } + + _sensor_hygrometer_pub.advertise(); + ScheduleOnInterval(50000); + return PX4_OK; +} + + + +int SHT3X::init_sensor() +{ + set_pointer(SHT3X_CMD_CLEAR_STATUS); + px4_usleep(2000); + + // Get sensor serial number + uint8_t serial[4]; + read_data(SHT3x_CMD_READ_SN, &serial[0], 6); + + _sht_info.serial_number = ((uint32_t)serial[0] << 24 + | (uint32_t)serial[1] << 16 + | (uint32_t)serial[2] << 8 + | (uint32_t)serial[3]); + + set_pointer(SHT3x_CMD_PERIODIC_2HZ_MEDIUM); + px4_usleep(2000); + probe(); + + PX4_INFO("Connected to SHT3x sensor, SN: %ld", _sht_info.serial_number); + + return PX4_OK; +} + +void SHT3X::RunImpl() +{ + switch (_state) { + case sht3x_state::INIT: + probe(); + init_sensor(); + _state = sht3x_state::MEASUREMENT; + break; + + case sht3x_state::MEASUREMENT: + if ((hrt_absolute_time() - measurement_time) > 200000) { + sensor_compouse_msg(1); + } + + if ((hrt_absolute_time() - measurement_time) > 3000000) { + _state = sht3x_state::ERROR_READOUT; + } + + break; + + case sht3x_state::ERROR_GENERAL: + case sht3x_state::ERROR_READOUT: { + if (_last_state != _state) { + PX4_INFO("I cant get new data. The sensor may be disconnected."); + } + + if (probe() == PX4_OK) { + _state = sht3x_state::INIT; + } + } + break; + } + + if (_last_state != _state) { + _time_in_state = hrt_absolute_time(); + _last_state = _state; + } +} + + +void +SHT3X::custom_method(const BusCLIArguments &cli) +{ + switch (cli.custom1) { + + case 1: { + PX4_INFO("Last measured values (%.3fs ago, #%d)", (double)(hrt_absolute_time() - measurement_time) / 1000000.0, + measurement_index); + PX4_INFO("Temp: %.3f, Hum: %.3f", (double)measured_temperature, (double)measured_humidity); + + } + break; + + case 2: { + _state = sht3x_state::INIT; + } + break; + } +} + + +void SHT3X::print_status() +{ + PX4_INFO("SHT3X sensor"); + I2CSPIDriverBase::print_status(); + PX4_INFO("SN: %ld", _sht_info.serial_number); + PX4_INFO("Status: %s", sht_state_names[_state]); +} + + +void SHT3X::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +SHT3x Temperature and Humidity Sensor Driver by Senserion. + +### Examples +CLI usage example: +$ sht3x start -X + Start the sensor driver on the external bus + +$ sht3x status + Print driver status + +$ sht3x values + Print last measured values + +$ sht3x reset + Reinitialize senzor, reset flags + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sht3x", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x44); + + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + PRINT_MODULE_USAGE_COMMAND_DESCR("values", "Print actual data"); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reinitialize sensor"); + +} + +int sht3x_main(int argc, char *argv[]) +{ + using ThisDriver = SHT3X; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + cli.i2c_address = 0x44; + cli.support_keep_running = true; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_HYGRO_DEVTYPE_SHT3X); + + 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); + } + + if (!strcmp(verb, "values")) { + cli.custom1 = 1; + return ThisDriver::module_custom_method(cli, iterator, false); + } + + if (!strcmp(verb, "reset")) { + cli.custom1 = 2; + return ThisDriver::module_custom_method(cli, iterator, false); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/hygrometer/sht3x/sht3x.h b/src/drivers/hygrometer/sht3x/sht3x.h new file mode 100644 index 000000000000..ff698446f0f3 --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x.h @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 sht3x.h + * + * Header file for SHT3x driver + * + * @author Roman Dvorak + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#define SHT3X_CMD_READ_STATUS 0xF32D +#define SHT3X_CMD_CLEAR_STATUS 0x3041 +#define SHT3x_CMD_PERIODIC_2HZ_MEDIUM 0x2220 +#define SHT3x_CMD_FETCH_DATA 0xE000 +#define SHT3x_CMD_READ_SN 0x3780 + + +using namespace time_literals; + +extern "C" __EXPORT int sht3x_main(int argc, char *argv[]); + +enum sht3x_state { + ERROR_GENERAL, + ERROR_READOUT, + INIT, + MEASUREMENT +}; +const char *sht_state_names[] = {"General error", "Readout error", "Initialization", + "Measurement" + }; + + +struct sht_info { + uint32_t serial_number; +}; + + +class SHT3X : public device::I2C, public ModuleParams, public I2CSPIDriver +{ +public: + SHT3X(const I2CSPIDriverConfig &config); + ~SHT3X() = default; + + static void print_usage(); + void RunImpl(); + + int init() override; + int probe() override; + int init_sensor(); + void print_status() override; + + void custom_method(const BusCLIArguments &cli); + + int set_pointer(uint16_t command); + int read_data(uint16_t command, uint8_t *data_ptr, uint8_t length); + int write_data(uint16_t command, uint8_t buffer[], uint8_t length); + + void sensor_compouse_msg(bool send); + + uint8_t calc_crc(uint8_t data[2]); + +private: + + float measured_temperature = 0; + float measured_humidity = 0; + uint32_t measurement_time = 0; + uint16_t measurement_index = 0; + + sht_info _sht_info; + int _state = sht3x_state::INIT; + int _last_state = sht3x_state::INIT; + uint32_t _time_in_state = hrt_absolute_time(); + uint16_t _last_command = 0; + uORB::PublicationMulti _sensor_hygrometer_pub{ORB_ID(sensor_hygrometer)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_en_sht3x + ) +}; diff --git a/src/drivers/hygrometer/sht3x/sht3x_params.c b/src/drivers/hygrometer/sht3x/sht3x_params.c new file mode 100644 index 000000000000..2d54886f4aee --- /dev/null +++ b/src/drivers/hygrometer/sht3x/sht3x_params.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * SHT3x temperature and hygrometer + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_SHT3X, 0); diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt deleted file mode 100644 index e512416babbe..000000000000 --- a/src/drivers/imu/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2017 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_subdirectory(adis16477) -add_subdirectory(adis16497) -add_subdirectory(analog_devices) -add_subdirectory(bosch) -add_subdirectory(fxas21002c) -add_subdirectory(fxos8701cq) -add_subdirectory(invensense) -add_subdirectory(l3gd20) -add_subdirectory(lsm303d) -add_subdirectory(st) diff --git a/src/drivers/imu/Kconfig b/src/drivers/imu/Kconfig new file mode 100644 index 000000000000..9a1de9995a17 --- /dev/null +++ b/src/drivers/imu/Kconfig @@ -0,0 +1,30 @@ +menu "IMU" + menuconfig COMMON_IMU + bool "Common IMU's" + default n + select DRIVERS_IMU_ADIS16477 + select DRIVERS_IMU_ADIS16497 + select DRIVERS_IMU_ANALOG_DEVICES_ADIS16448 + select DRIVERS_IMU_ANALOG_DEVICES_ADIS16470 + select DRIVERS_IMU_BOSCH_BMI055 + select DRIVERS_IMU_BOSCH_BMI088 + select DRIVERS_IMU_FXAS21002C + select DRIVERS_IMU_FXOS8701CQ + select DRIVERS_IMU_INVENSENSE_ICM20602 + select DRIVERS_IMU_INVENSENSE_ICM20608G + select DRIVERS_IMU_INVENSENSE_ICM20649 + select DRIVERS_IMU_INVENSENSE_ICM20689 + #select DRIVERS_IMU_INVENSENSE_ICM20948 + select DRIVERS_IMU_INVENSENSE_ICM40609D + select DRIVERS_IMU_INVENSENSE_ICM42605 + select DRIVERS_IMU_INVENSENSE_ICM42688P + select DRIVERS_IMU_INVENSENSE_MPU6000 + select DRIVERS_IMU_INVENSENSE_MPU6500 + select DRIVERS_IMU_INVENSENSE_MPU9250 + select DRIVERS_IMU_L3GD20 + select DRIVERS_IMU_LSM303D + select DRIVERS_IMU_ST + ---help--- + Enable default set of IMU drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index e0eec84b4ad9..98c9787eac43 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -53,15 +53,14 @@ static constexpr uint32_t ADIS16477_DEFAULT_RATE = 1000; using namespace time_literals; -ADIS16477::ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ADIS16477, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation), +ADIS16477::ADIS16477(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), - _drdy_gpio(drdy_gpio) + _drdy_gpio(config.drdy_gpio) { #ifdef GPIO_SPI1_RESET_ADIS16477 // Configure hardware reset line diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index 9a4d7ae80ecd..c99de49d6b37 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include #include @@ -50,12 +50,9 @@ class ADIS16477 : public device::SPI, public I2CSPIDriver { public: - ADIS16477(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ADIS16477(const I2CSPIDriverConfig &config); virtual ~ADIS16477(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init(); diff --git a/src/drivers/imu/adis16477/Kconfig b/src/drivers/imu/adis16477/Kconfig new file mode 100644 index 000000000000..74b76d24824e --- /dev/null +++ b/src/drivers/imu/adis16477/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ADIS16477 + bool "adis16477" + default n + ---help--- + Enable support for adis16477 \ No newline at end of file diff --git a/src/drivers/imu/adis16477/adis16477_main.cpp b/src/drivers/imu/adis16477/adis16477_main.cpp index 3b5b94a86336..d58f72e6dfa5 100644 --- a/src/drivers/imu/adis16477/adis16477_main.cpp +++ b/src/drivers/imu/adis16477/adis16477_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019, 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 @@ -47,25 +47,6 @@ ADIS16477::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ADIS16477::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ADIS16477 *instance = new ADIS16477(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int adis16477_main(int argc, char *argv[]) { int ch; @@ -73,15 +54,15 @@ extern "C" int adis16477_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = 1000000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 6d5396fb9257..9b39f4a07101 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -70,15 +70,14 @@ static constexpr uint32_t ADIS16497_DEFAULT_RATE = 1000; using namespace time_literals; -ADIS16497::ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ADIS16497, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation), +ADIS16497::ADIS16497(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), - _drdy_gpio(drdy_gpio) + _drdy_gpio(config.drdy_gpio) { #ifdef GPIO_SPI1_RESET_ADIS16497 // Configure hardware reset line diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp index a3f6e304733f..828e53697c23 100644 --- a/src/drivers/imu/adis16497/ADIS16497.hpp +++ b/src/drivers/imu/adis16497/ADIS16497.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include #include @@ -86,12 +86,9 @@ static constexpr uint32_t crc32_tab[] = { class ADIS16497 : public device::SPI, public I2CSPIDriver { public: - ADIS16497(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ADIS16497(const I2CSPIDriverConfig &config); virtual ~ADIS16497(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; diff --git a/src/drivers/imu/adis16497/Kconfig b/src/drivers/imu/adis16497/Kconfig new file mode 100644 index 000000000000..41fa1b759bda --- /dev/null +++ b/src/drivers/imu/adis16497/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ADIS16497 + bool "adis16497" + default n + ---help--- + Enable support for adis16497 \ No newline at end of file diff --git a/src/drivers/imu/adis16497/adis16497_main.cpp b/src/drivers/imu/adis16497/adis16497_main.cpp index 7f993fa912d1..d7e16832a9bb 100644 --- a/src/drivers/imu/adis16497/adis16497_main.cpp +++ b/src/drivers/imu/adis16497/adis16497_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019, 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 @@ -47,25 +47,6 @@ ADIS16497::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ADIS16497::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ADIS16497 *instance = new ADIS16497(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int adis16497_main(int argc, char *argv[]) { int ch; @@ -73,15 +54,15 @@ extern "C" int adis16497_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = 5000000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/analog_devices/Kconfig b/src/drivers/imu/analog_devices/Kconfig new file mode 100644 index 000000000000..15f0287c18c2 --- /dev/null +++ b/src/drivers/imu/analog_devices/Kconfig @@ -0,0 +1,3 @@ +menu "Analog Devices" +rsource "*/Kconfig" +endmenu diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index 9766871f744a..5566a60db76a 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -95,24 +95,26 @@ static int16_t convert12BitToINT16(uint16_t word) return output; } -ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, SPIDEV_MODE3, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), // TODO: DRDY disabled - _px4_accel(get_device_id(), rotation), - _px4_baro(get_device_id()), - _px4_gyro(get_device_id(), rotation), - _px4_mag(get_device_id(), rotation) +ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), // TODO: DRDY disabled + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation), + _px4_mag(get_device_id(), config.rotation) { + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } } ADIS16448::~ADIS16448() { perf_free(_reset_perf); - perf_free(_perf_crc_bad); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); + perf_free(_perf_crc_bad); + perf_free(_drdy_missed_perf); } int ADIS16448::init() @@ -147,9 +149,10 @@ void ADIS16448::print_status() I2CSPIDriverBase::print_status(); perf_print_counter(_reset_perf); - perf_print_counter(_perf_crc_bad); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_drdy_missed_perf); } int ADIS16448::probe() @@ -159,25 +162,24 @@ int ADIS16448::probe() PX4_WARN("Power-On Start-Up Time is 205 ms"); } - const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + for (int attempt = 0; attempt < 3; attempt++) { + const uint16_t PROD_ID = RegisterReadVerified(Register::PROD_ID); - if (PROD_ID != Product_identification) { - DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); - return PX4_ERROR; - } + if (PROD_ID == Product_identification) { + const uint16_t SERIAL_NUM = RegisterReadVerified(Register::SERIAL_NUM); + const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1); + const uint16_t LOT_ID2 = RegisterReadVerified(Register::LOT_ID2); - const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); - const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); - const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); + PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); - PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); + return PX4_OK; - // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) - if (LOT_ID1 == 0x1824) { - _check_crc = true; + } else { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + } } - return PX4_OK; + return PX4_ERROR; } void ADIS16448::RunImpl() @@ -198,7 +200,7 @@ void ADIS16448::RunImpl() case STATE::WAIT_FOR_RESET: if (_self_test_passed) { - if ((RegisterRead(Register::PROD_ID) == Product_identification)) { + if ((RegisterReadVerified(Register::PROD_ID) == Product_identification)) { // if reset succeeded then configure _state = STATE::CONFIGURE; ScheduleNow(); @@ -211,21 +213,41 @@ void ADIS16448::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } } else { RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test); _state = STATE::SELF_TEST_CHECK; - ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms + ScheduleDelayed(90_ms); // Automatic Self-Test Time > 45 ms } break; case STATE::SELF_TEST_CHECK: { - const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); + const uint16_t MSC_CTRL = RegisterReadVerified(Register::MSC_CTRL); + + if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) { + // self test not finished, check again + if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) { + ScheduleDelayed(45_ms); + PX4_DEBUG("self test not complete, check again in 45 ms"); + return; + + } else { + // still not cleared, fail self test + _self_test_passed = false; + _state = STATE::RESET; + ScheduleDelayed(1000_ms); + return; + } + } + + bool test_passed = true; + + const uint16_t DIAG_STAT = RegisterReadVerified(Register::DIAG_STAT); if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { PX4_ERR("self test failed"); @@ -248,6 +270,7 @@ void ADIS16448::RunImpl() if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { PX4_ERR("gyroscope self-test failure"); + test_passed = false; } // Accelerometer @@ -257,18 +280,20 @@ void ADIS16448::RunImpl() if (accel_x_fail || accel_y_fail || accel_z_fail) { PX4_ERR("accelerometer self-test failure"); + test_passed = false; } + } - _self_test_passed = false; - _state = STATE::RESET; - ScheduleDelayed(1000_ms); - - } else { + if (test_passed) { PX4_DEBUG("self test passed"); _self_test_passed = true; - _state = STATE::RESET; - ScheduleNow(); + + } else { + _self_test_passed = false; } + + _state = STATE::RESET; + ScheduleDelayed(10_ms); } break; @@ -305,7 +330,19 @@ void ADIS16448::RunImpl() break; case STATE::READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + // push backup schedule back ScheduleDelayed(SAMPLE_INTERVAL_US * 2); } @@ -364,18 +401,38 @@ void ADIS16448::RunImpl() _px4_accel.set_temperature(temperature); _px4_gyro.set_temperature(temperature); + bool imu_updated = false; + // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) const int16_t accel_x = buffer.XACCL_OUT; const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; + if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) { + imu_updated = true; + + _accel_prev[0] = accel_x; + _accel_prev[1] = accel_y; + _accel_prev[2] = accel_z; + } + const int16_t gyro_x = buffer.XGYRO_OUT; const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; - _px4_accel.update(now, accel_x, accel_y, accel_z); - _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) { + imu_updated = true; + + _gyro_prev[0] = gyro_x; + _gyro_prev[1] = gyro_y; + _gyro_prev[2] = gyro_z; + } + + if (imu_updated) { + _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); + _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); + } // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) { @@ -385,13 +442,20 @@ void ADIS16448::RunImpl() const int16_t mag_x = buffer.XMAGN_OUT; const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT; const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT; - _px4_mag.update(now, mag_x, mag_y, mag_z); + _px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z); - _px4_baro.set_error_count(error_count); - _px4_baro.set_temperature(temperature); float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB - _px4_baro.update(now, pressure_pa); + + // publish baro + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = get_device_id(); + sensor_baro.pressure = pressure_pa; + sensor_baro.temperature = temperature; + sensor_baro.error_count = error_count; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); } success = true; @@ -435,6 +499,27 @@ void ADIS16448::RunImpl() bool ADIS16448::Configure() { + const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1); + + // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) + if (LOT_ID1 == 0x1824) { + _check_crc = true; + + if (_perf_crc_bad == nullptr) { + _perf_crc_bad = perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"); + } + + } else { + _check_crc = false; + + for (auto ®_cfg : _register_cfg) { + if (reg_cfg.reg == Register::MSC_CTRL) { + reg_cfg.set_bits = reg_cfg.set_bits & ~MSC_CTRL_BIT::CRC16_for_burst; + break; + } + } + } + // first set and clear all configured register bits for (const auto ®_cfg : _register_cfg) { RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); @@ -456,8 +541,6 @@ bool ADIS16448::Configure() _px4_accel.set_range(18.f * CONSTANTS_ONE_G); _px4_gyro.set_range(math::radians(1000.f)); - _px4_mag.set_external(external()); - return success; } @@ -540,7 +623,7 @@ bool ADIS16448::RegisterCheck(const register_config_t ®_cfg) { bool success = true; - const uint16_t reg_value = RegisterRead(reg_cfg.reg); + const uint16_t reg_value = RegisterReadVerified(reg_cfg.reg); if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); @@ -565,10 +648,26 @@ uint16_t ADIS16448::RegisterRead(Register reg) transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(nullptr, cmd, 1); + px4_udelay(SPI_STALL_PERIOD); return cmd[0]; } +uint16_t ADIS16448::RegisterReadVerified(Register reg, int retries) +{ + for (int attempt = 0; attempt < retries; attempt++) { + uint16_t read1 = RegisterRead(reg); + uint16_t read2 = RegisterRead(reg); + + if (read1 == read2) { + return read1; + } + } + + // failed + return 0; +} + void ADIS16448::RegisterWrite(Register reg, uint16_t value) { set_frequency(SPI_SPEED); @@ -580,11 +679,12 @@ void ADIS16448::RegisterWrite(Register reg, uint16_t value) transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(cmd + 1, nullptr, 1); + px4_udelay(SPI_STALL_PERIOD); } void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) { - const uint16_t orig_val = RegisterRead(reg); + const uint16_t orig_val = RegisterReadVerified(reg); uint16_t val = (orig_val & ~clearbits) | setbits; diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index 4987fef32bf6..a28f6f270654 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -45,10 +45,11 @@ #include #include #include -#include #include #include -#include +#include +#include +#include #include #include #include @@ -58,12 +59,9 @@ using namespace Analog_Devices_ADIS16448; class ADIS16448 : public device::SPI, public I2CSPIDriver { public: - ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_drdy_gpio_t drdy_gpio); + ADIS16448(const I2CSPIDriverConfig &config); ~ADIS16448() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -94,30 +92,37 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver bool RegisterCheck(const register_config_t ®_cfg); uint16_t RegisterRead(Register reg); + uint16_t RegisterReadVerified(Register reg, int retries = 1); void RegisterWrite(Register reg, uint16_t value); void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); const spi_drdy_gpio_t _drdy_gpio; PX4Accelerometer _px4_accel; - PX4Barometer _px4_baro; PX4Gyroscope _px4_gyro; PX4Magnetometer _px4_mag; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _perf_crc_bad{nullptr}; + perf_counter_t _drdy_missed_perf{nullptr}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; + bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) bool _self_test_passed{false}; + int16_t _accel_prev[3] {}; + int16_t _gyro_prev[3] {}; + enum class STATE : uint8_t { RESET, WAIT_FOR_RESET, @@ -127,12 +132,11 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver } _state{STATE::RESET}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{4}; + static constexpr uint8_t size_register_cfg{3}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 }, { Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate }, - { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B }, - { Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0}, + { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set | SENS_AVG_BIT::Filter_Size_Variable_B_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B_clear }, }; }; diff --git a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp index a3d221d36ca2..d671cde77f8a 100644 --- a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp +++ b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp @@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read -static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data +static constexpr uint32_t SPI_STALL_PERIOD = 11; // 9 us Stall period between data static constexpr uint16_t DIR_WRITE = 0x80; @@ -128,8 +128,8 @@ enum GLOB_CMD_BIT : uint16_t { // SMPL_PRD enum SMPL_PRD_BIT : uint16_t { - // [12:8] D, decimation rate setting, binomial, - decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable + // [12:8] D, decimation rate setting, binomial + decimation_rate = Bit12 | Bit11 | Bit10 | Bit9 | Bit8, // disable internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS }; @@ -141,8 +141,8 @@ enum SENS_AVG_BIT : uint16_t { Measurement_range_1000_clear = Bit9 | Bit8, // [2:0] Filter Size Variable B - Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable - + Filter_Size_Variable_B_set = Bit1, + Filter_Size_Variable_B_clear = Bit2 | Bit0, }; // GPIO_CTRL diff --git a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt index ed488387efbb..b8f8d690fcf3 100644 --- a/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt +++ b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt @@ -44,7 +44,6 @@ px4_add_module( Analog_Devices_ADIS16448_registers.hpp DEPENDS drivers_accelerometer - drivers_barometer drivers_gyroscope drivers_magnetometer px4_work_queue diff --git a/src/drivers/imu/analog_devices/adis16448/Kconfig b/src/drivers/imu/analog_devices/adis16448/Kconfig new file mode 100644 index 000000000000..2b5e1a9a2a76 --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ANALOG_DEVICES_ADIS16448 + bool "ADIS16448" + default n + ---help--- + Enable support for analog_devices ADIS16448 diff --git a/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp index b04c9d6403f6..55489ce3f940 100644 --- a/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp +++ b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp @@ -46,25 +46,6 @@ void ADIS16448::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ADIS16448::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ADIS16448 *instance = new ADIS16448(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int adis16448_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int adis16448_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/analog_devices/adis16448/parameters.c b/src/drivers/imu/analog_devices/adis16448/parameters.c index 9b44b9e2c42e..9b3373609252 100644 --- a/src/drivers/imu/analog_devices/adis16448/parameters.c +++ b/src/drivers/imu/analog_devices/adis16448/parameters.c @@ -42,3 +42,15 @@ * @value 1 Enabled */ PARAM_DEFINE_INT32(SENS_EN_ADIS164X, 0); + +/** + * Analog Devices ADIS16448 IMU Orientation(external SPI) + * + * @reboot_required true + * @min 0 + * @max 101 + * @group Sensors + * @value 0 ROTATION_NONE + * @value 4 ROTATION_YAW_180 + */ +PARAM_DEFINE_INT32(SENS_OR_ADIS164X, 0); diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 6c02e562744b..f978513b803b 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -40,21 +40,25 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ADIS16470::ADIS16470(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ADIS16470, MODULE_NAME, bus, device, SPIDEV_MODE3, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ADIS16470::ADIS16470(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - _debug_enabled = true; + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } } ADIS16470::~ADIS16470() { + perf_free(_reset_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); + perf_free(_perf_crc_bad); + perf_free(_drdy_missed_perf); } int ADIS16470::init() @@ -89,9 +93,10 @@ void ADIS16470::print_status() I2CSPIDriverBase::print_status(); perf_print_counter(_reset_perf); - perf_print_counter(_perf_crc_bad); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_drdy_missed_perf); } int ADIS16470::probe() @@ -210,7 +215,19 @@ void ADIS16470::RunImpl() break; case STATE::READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + // push backup schedule back ScheduleDelayed(SAMPLE_INTERVAL_US * 2); } @@ -235,7 +252,6 @@ void ADIS16470::RunImpl() static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits"); buffer.cmd = static_cast(Register::GLOB_CMD) << 8; - set_frequency(SPI_SPEED_BURST); if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { @@ -293,7 +309,7 @@ void ADIS16470::RunImpl() accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; - _px4_accel.update(now, accel_x, accel_y, accel_z); + _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); int16_t gyro_x = buffer.X_GYRO_OUT; @@ -303,7 +319,7 @@ void ADIS16470::RunImpl() // flip y & z to publish right handed with z down (x forward, y right, z down) gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; - _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); success = true; @@ -361,7 +377,7 @@ bool ADIS16470::Configure() _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); _px4_accel.set_range(40.f * CONSTANTS_ONE_G); - _px4_gyro.set_scale(math::radians(2000.f / 32768.f)); + _px4_gyro.set_scale(math::radians(1.f / 0.1f)); // 1 LSB = 0.1°/sec _px4_gyro.set_range(math::radians(2000.f)); _px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB @@ -378,6 +394,7 @@ int ADIS16470::DataReadyInterruptCallback(int irq, void *context, void *arg) void ADIS16470::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp index 9577495baea4..40c6fab14f9f 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp @@ -1,35 +1,35 @@ /**************************************************************************** -* -* 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. -* -****************************************************************************/ + * + * 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 ADIS16470.hpp @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace Analog_Devices_ADIS16470; class ADIS16470 : public device::SPI, public I2CSPIDriver { public: - ADIS16470(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_drdy_gpio_t drdy_gpio); + ADIS16470(const I2CSPIDriverConfig &config); ~ADIS16470() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -101,15 +98,18 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver PX4Gyroscope _px4_gyro; perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; + perf_counter_t _drdy_missed_perf{nullptr}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; + bool _self_test_passed{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/analog_devices/adis16470/Kconfig b/src/drivers/imu/analog_devices/adis16470/Kconfig new file mode 100644 index 000000000000..9238209a785b --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16470/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ANALOG_DEVICES_ADIS16470 + bool "ADIS16470" + default n + ---help--- + Enable support for analog_devices ADIS16470 diff --git a/src/drivers/imu/analog_devices/adis16470/adis16470_main.cpp b/src/drivers/imu/analog_devices/adis16470/adis16470_main.cpp index ba12fdf9476b..079cd2535cfb 100644 --- a/src/drivers/imu/analog_devices/adis16470/adis16470_main.cpp +++ b/src/drivers/imu/analog_devices/adis16470/adis16470_main.cpp @@ -46,25 +46,6 @@ void ADIS16470::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ADIS16470::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ADIS16470 *instance = new ADIS16470(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int adis16470_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int adis16470_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/bosch/Kconfig b/src/drivers/imu/bosch/Kconfig new file mode 100644 index 000000000000..ec6fdc279578 --- /dev/null +++ b/src/drivers/imu/bosch/Kconfig @@ -0,0 +1,3 @@ +menu "Bosch" +rsource "*/Kconfig" +endmenu diff --git a/src/drivers/imu/bosch/bmi055/BMI055.cpp b/src/drivers/imu/bosch/bmi055/BMI055.cpp index 4b6582adebcd..dc11e1b96b3d 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.cpp @@ -36,18 +36,15 @@ #include "BMI055_Accelerometer.hpp" #include "BMI055_Gyroscope.hpp" -I2CSPIDriverBase *BMI055::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *BMI055::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { BMI055 *instance = nullptr; - if (cli.type == DRV_ACC_DEVTYPE_BMI055) { - instance = new Bosch::BMI055::Accelerometer::BMI055_Accelerometer(iterator.configuredBusOption(), iterator.bus(), - iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI055) { + instance = new Bosch::BMI055::Accelerometer::BMI055_Accelerometer(config); - } else if (cli.type == DRV_GYR_DEVTYPE_BMI055) { - instance = new Bosch::BMI055::Gyroscope::BMI055_Gyroscope(iterator.configuredBusOption(), iterator.bus(), - iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI055) { + instance = new Bosch::BMI055::Gyroscope::BMI055_Gyroscope(config); } if (!instance) { @@ -63,11 +60,10 @@ I2CSPIDriverBase *BMI055::instantiate(const BusCLIArguments &cli, const BusInsta return instance; } -BMI055::BMI055(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, - enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) : - SPI(devtype, name, bus, device, mode, frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype), - _drdy_gpio(drdy_gpio) +BMI055::BMI055(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) { } diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp index af0e857ccf0b..f4fad2772ea2 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -43,13 +43,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) class BMI055 : public device::SPI, public I2CSPIDriver { public: - BMI055(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode, - uint32_t frequency, spi_drdy_gpio_t drdy_gpio); + BMI055(const I2CSPIDriverConfig &config); virtual ~BMI055() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual void RunImpl() = 0; @@ -68,7 +66,7 @@ class BMI055 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -76,9 +74,7 @@ class BMI055 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 27ed70a0ac84..77cf53730283 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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,19 +33,18 @@ #include "BMI055_Accelerometer.hpp" -#include // CONSTANTS_ONE_G +#include // CONSTANTS_ONE_G using namespace time_literals; namespace Bosch::BMI055::Accelerometer { -BMI055_Accelerometer::BMI055_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI055(DRV_ACC_DEVTYPE_BMI055, "BMI055_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_accel(get_device_id(), rotation) +BMI055_Accelerometer::BMI055_Accelerometer(const I2CSPIDriverConfig &config) : + BMI055(config), + _px4_accel(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } @@ -163,9 +162,16 @@ void BMI055_Accelerometer::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -193,7 +199,20 @@ void BMI055_Accelerometer::RunImpl() perf_count(_fifo_empty_perf); } else if (fifo_frame_counter >= 1) { - if (FIFORead(now, fifo_frame_counter)) { + + uint8_t samples = fifo_frame_counter; + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + // sample timestamp set from data ready already corresponds to _fifo_samples + if (timestamp_sample == 0) { + timestamp_sample = now - static_cast(FIFO_SAMPLE_DT); + } + + samples--; + } + + if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -243,22 +262,22 @@ void BMI055_Accelerometer::ConfigureAccel() const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0); switch (PMU_RANGE) { - case range_2g: + case range_2g_set: _px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB _px4_accel.set_range(2.f * CONSTANTS_ONE_G); break; - case range_4g: + case range_4g_set: _px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB _px4_accel.set_range(4.f * CONSTANTS_ONE_G); break; - case range_8g: + case range_8g_set: _px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB _px4_accel.set_range(8.f * CONSTANTS_ONE_G); break; - case range_16g: + case range_16g_set: _px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB _px4_accel.set_range(16.f * CONSTANTS_ONE_G); break; @@ -319,11 +338,8 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI055_Accelerometer::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool BMI055_Accelerometer::DataReadyInterruptConfigure() @@ -438,7 +454,7 @@ void BMI055_Accelerometer::FIFOReset() RegisterWrite(Register::FIFO_CONFIG_1, 0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp index 80ef7831b203..31906c8d96bd 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI055::Accelerometer class BMI055_Accelerometer : public BMI055 { public: - BMI055_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI055_Accelerometer(const I2CSPIDriverConfig &config); ~BMI055_Accelerometer() override; void RunImpl() override; @@ -59,7 +58,7 @@ class BMI055_Accelerometer : public BMI055 static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { @@ -113,7 +112,7 @@ class BMI055_Accelerometer : public BMI055 static constexpr uint8_t size_register_cfg{7}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::PMU_RANGE, PMU_RANGE_BIT::range_16g, Bit1 | Bit0}, + { Register::PMU_RANGE, PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear}, { Register::ACCD_HBW, ACCD_HBW_BIT::data_high_bw, 0}, { Register::INT_EN_1, INT_EN_1_BIT::int_fwm_en, 0}, { Register::INT_MAP_1, INT_MAP_1_BIT::int1_fwm, 0}, diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index 6a18c2a7c5fe..d2ea51453003 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,12 +40,11 @@ using namespace time_literals; namespace Bosch::BMI055::Gyroscope { -BMI055_Gyroscope::BMI055_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI055(DRV_GYR_DEVTYPE_BMI055, "BMI055_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_gyro(get_device_id(), rotation) +BMI055_Gyroscope::BMI055_Gyroscope(const I2CSPIDriverConfig &config) : + BMI055(config), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } @@ -163,9 +162,16 @@ void BMI055_Gyroscope::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -193,7 +199,20 @@ void BMI055_Gyroscope::RunImpl() perf_count(_fifo_empty_perf); } else if (fifo_frame_counter >= 1) { - if (FIFORead(now, fifo_frame_counter)) { + + uint8_t samples = fifo_frame_counter; + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + // sample timestamp set from data ready already corresponds to _fifo_samples + if (timestamp_sample == 0) { + timestamp_sample = now - static_cast(FIFO_SAMPLE_DT); + } + + samples--; + } + + if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -317,11 +336,8 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI055_Gyroscope::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool BMI055_Gyroscope::DataReadyInterruptConfigure() @@ -435,7 +451,7 @@ void BMI055_Gyroscope::FIFOReset() RegisterWrite(Register::FIFO_CONFIG_1, 0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp index 54331abdc263..13c0468f0d74 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI055::Gyroscope class BMI055_Gyroscope : public BMI055 { public: - BMI055_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI055_Gyroscope(const I2CSPIDriverConfig &config); ~BMI055_Gyroscope() override; void RunImpl() override; @@ -59,7 +58,7 @@ class BMI055_Gyroscope : public BMI055 static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp index 697d2484796a..f95669f29382 100644 --- a/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp +++ b/src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -97,10 +97,17 @@ enum ACCD_HBW_BIT : uint8_t { // PMU_RANGE enum PMU_RANGE_BIT : uint8_t { // range<3:0> - range_2g = Bit1 | Bit0, // ́0011b ́ -> ±2g range - range_4g = Bit2 | Bit0, // ́0101b ́ -> ±4g range - range_8g = Bit3, // ́1000b ́ -> ±8g range - range_16g = Bit3 | Bit2, // ́1100b ́ -> ±16g range + range_16g_set = Bit3 | Bit2, // ́1100b ́ -> ±16g range + range_16g_clear = Bit1 | Bit0, + + range_8g_set = Bit3, // ́1000b ́ -> ±8g range + range_8g_clear = Bit2 | Bit1 | Bit0, + + range_4g_set = Bit2 | Bit0, // ́0101b ́ -> ±4g range + range_4g_clear = Bit3 | Bit1, + + range_2g_set = Bit1 | Bit0, // ́0011b ́ -> ±2g range + range_2g_clear = Bit3 | Bit2, }; // INT_EN_1 diff --git a/src/drivers/imu/bosch/bmi055/Kconfig b/src/drivers/imu/bosch/bmi055/Kconfig new file mode 100644 index 000000000000..473a97021bb1 --- /dev/null +++ b/src/drivers/imu/bosch/bmi055/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_BOSCH_BMI055 + bool "bosch bmi055" + default n + ---help--- + Enable support for bosch bmi055 diff --git a/src/drivers/imu/bosch/bmi055/bmi055_main.cpp b/src/drivers/imu/bosch/bmi055/bmi055_main.cpp index 3a664e5d7d6f..62ff574675b7 100644 --- a/src/drivers/imu/bosch/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bosch/bmi055/bmi055_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -53,33 +53,36 @@ extern "C" int bmi055_main(int argc, char *argv[]) int ch; using ThisDriver = BMI055; BusCLIArguments cli{false, true}; - cli.type = 0; + uint16_t type = 0; cli.default_spi_frequency = 10000000; + const char *name = MODULE_NAME; - while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) { switch (ch) { case 'A': - cli.type = DRV_ACC_DEVTYPE_BMI055; + type = DRV_ACC_DEVTYPE_BMI055; + name = MODULE_NAME "_accel"; break; case 'G': - cli.type = DRV_GYR_DEVTYPE_BMI055; + type = DRV_GYR_DEVTYPE_BMI055; + name = MODULE_NAME "_gyro"; break; case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); - if (!verb) { + if (!verb || type == 0) { ThisDriver::print_usage(); return -1; } - BusInstanceIterator iterator(MODULE_NAME, cli, cli.type); + BusInstanceIterator iterator(name, cli, type); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); diff --git a/src/drivers/imu/bosch/bmi088/BMI088.cpp b/src/drivers/imu/bosch/bmi088/BMI088.cpp index e5cb4d69f93f..0c45fd3a2d67 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -36,18 +36,15 @@ #include "BMI088_Accelerometer.hpp" #include "BMI088_Gyroscope.hpp" -I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *BMI088::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { BMI088 *instance = nullptr; - if (cli.type == DRV_ACC_DEVTYPE_BMI088) { - instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(), - iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(config); - } else if (cli.type == DRV_GYR_DEVTYPE_BMI088) { - instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(), - iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(config); } if (!instance) { @@ -63,11 +60,10 @@ I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInsta return instance; } -BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, - enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) : - SPI(devtype, name, bus, device, mode, frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype), - _drdy_gpio(drdy_gpio) +BMI088::BMI088(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) { } diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp index 7f750feebfd3..35187fdb7203 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -43,13 +43,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) class BMI088 : public device::SPI, public I2CSPIDriver { public: - BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode, - uint32_t frequency, spi_drdy_gpio_t drdy_gpio); + BMI088(const I2CSPIDriverConfig &config); virtual ~BMI088() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual void RunImpl() = 0; @@ -68,7 +66,7 @@ class BMI088 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -76,9 +74,7 @@ class BMI088 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index f8d900eb57f7..8f0dc0068b69 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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,19 +33,18 @@ #include "BMI088_Accelerometer.hpp" -#include // CONSTANTS_ONE_G +#include // CONSTANTS_ONE_G using namespace time_literals; namespace Bosch::BMI088::Accelerometer { -BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_accel(get_device_id(), rotation) +BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) : + BMI088(config), + _px4_accel(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } @@ -185,15 +184,19 @@ void BMI088_Accelerometer::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; samples = _fifo_samples; + + } else { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -215,6 +218,12 @@ void BMI088_Accelerometer::RunImpl() } else { samples = fifo_byte_counter / sizeof(FIFO::DATA); + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); @@ -227,7 +236,7 @@ void BMI088_Accelerometer::RunImpl() bool success = false; if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -278,22 +287,22 @@ void BMI088_Accelerometer::ConfigureAccel() switch (ACC_RANGE) { case acc_range_3g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(3.f); + _px4_accel.set_range(3.f * CONSTANTS_ONE_G); break; case acc_range_6g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(6.f); + _px4_accel.set_range(6.f * CONSTANTS_ONE_G); break; case acc_range_12g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(12.f); + _px4_accel.set_range(12.f * CONSTANTS_ONE_G); break; case acc_range_24g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(24.f); + _px4_accel.set_range(24.f * CONSTANTS_ONE_G); break; } } @@ -361,11 +370,8 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI088_Accelerometer::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool BMI088_Accelerometer::DataReadyInterruptConfigure() @@ -562,7 +568,7 @@ void BMI088_Accelerometer::FIFOReset() RegisterWrite(Register::ACC_SOFTRESET, 0xB0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } void BMI088_Accelerometer::UpdateTemperature() diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp index a7818e60ddfd..ccf5f40d3a56 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI088::Accelerometer class BMI088_Accelerometer : public BMI088 { public: - BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI088_Accelerometer(const I2CSPIDriverConfig &config); ~BMI088_Accelerometer() override; void RunImpl() override; @@ -59,7 +58,7 @@ class BMI088_Accelerometer : public BMI088 static constexpr uint32_t RATE{1600}; // 1600 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index ef8e6a759e1c..7dac23e578b7 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,12 +40,11 @@ using namespace time_literals; namespace Bosch::BMI088::Gyroscope { -BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_gyro(get_device_id(), rotation) +BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) : + BMI088(config), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } @@ -164,9 +163,16 @@ void BMI088_Gyroscope::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -194,7 +200,20 @@ void BMI088_Gyroscope::RunImpl() perf_count(_fifo_empty_perf); } else if (fifo_frame_counter >= 1) { - if (FIFORead(now, fifo_frame_counter)) { + + uint8_t samples = fifo_frame_counter; + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + // sample timestamp set from data ready already corresponds to _fifo_samples + if (timestamp_sample == 0) { + timestamp_sample = now - static_cast(FIFO_SAMPLE_DT); + } + + samples--; + } + + if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -318,11 +337,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI088_Gyroscope::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool BMI088_Gyroscope::DataReadyInterruptConfigure() @@ -436,7 +452,7 @@ void BMI088_Gyroscope::FIFOReset() RegisterWrite(Register::FIFO_CONFIG_1, 0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp index dc6aa32b3b67..59d847bf02ba 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI088::Gyroscope class BMI088_Gyroscope : public BMI088 { public: - BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI088_Gyroscope(const I2CSPIDriverConfig &config); ~BMI088_Gyroscope() override; void RunImpl() override; @@ -59,7 +58,7 @@ class BMI088_Gyroscope : public BMI088 static constexpr uint32_t RATE{2000}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { 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 5aca22dba6b5..00b8ffe6d66a 100644 --- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 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 42d9df8f4089..0f1974f2cba4 100644 --- a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 diff --git a/src/drivers/imu/bosch/bmi088/Kconfig b/src/drivers/imu/bosch/bmi088/Kconfig new file mode 100644 index 000000000000..6985ccb7eb75 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_BOSCH_BMI088 + bool "bosch bmi088" + default n + ---help--- + Enable support for bosch bmi088 diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp b/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp deleted file mode 100644 index 1e00b839caa4..000000000000 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp +++ /dev/null @@ -1,95 +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 "BMI088.hpp" - -#include "BMI088_Accelerometer.hpp" -#include "BMI088_Gyroscope.hpp" - -I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - BMI088 *instance = nullptr; - - if (cli.type == DRV_ACC_DEVTYPE_BMI088) { - instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(), - cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - } else if (cli.type == DRV_GYR_DEVTYPE_BMI088) { - instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(), - cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - } - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - -BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, - enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) : - I2C(devtype, name, bus, device, frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype), - _drdy_gpio(drdy_gpio) -{ -} - -int BMI088::init() -{ - int ret = I2C::init(); - - if (ret != PX4_OK) { - DEVICE_DEBUG("I2C::init failed (%i)", ret); - return ret; - } - - int res = Reset() ? 0 : -1; - _state = STATE::SELFTEST; - - return res; -} - -bool BMI088::Reset() -{ - _state = STATE::RESET; - ScheduleClear(); - ScheduleNow(); - return true; -} diff --git a/src/drivers/imu/bosch/bmi088/bmi088_main.cpp b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp index e6949e4cf649..182c1e90d49d 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_main.cpp +++ b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -53,33 +53,36 @@ extern "C" int bmi088_main(int argc, char *argv[]) int ch; using ThisDriver = BMI088; BusCLIArguments cli{false, true}; - cli.type = 0; + uint16_t type = 0; cli.default_spi_frequency = 10000000; + const char *name = MODULE_NAME; - while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) { switch (ch) { case 'A': - cli.type = DRV_ACC_DEVTYPE_BMI088; + type = DRV_ACC_DEVTYPE_BMI088; + name = MODULE_NAME "_accel"; break; case 'G': - cli.type = DRV_GYR_DEVTYPE_BMI088; + type = DRV_GYR_DEVTYPE_BMI088; + name = MODULE_NAME "_gyro"; break; case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); - if (!verb) { + if (!verb || type == 0) { ThisDriver::print_usage(); return -1; } - BusInstanceIterator iterator(MODULE_NAME, cli, cli.type); + BusInstanceIterator iterator(name, cli, type); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp new file mode 100644 index 000000000000..54a8b2eed2bd --- /dev/null +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 "BMI088.hpp" + +#include "BMI088_Accelerometer.hpp" +#include "BMI088_Gyroscope.hpp" + +I2CSPIDriverBase *BMI088::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + BMI088 *instance = nullptr; + + if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(config); + + } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(config); + } + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +BMI088::BMI088(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) +{ +} + +int BMI088::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + int res = Reset() ? 0 : -1; + _state = STATE::SELFTEST; + + return res; +} + +bool BMI088::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp similarity index 85% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp rename to src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp index 1acf72e4e57d..6c1734a059db 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -43,13 +43,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) class BMI088 : public device::I2C, public I2CSPIDriver { public: - BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode, - uint32_t frequency, spi_drdy_gpio_t drdy_gpio); + BMI088(const I2CSPIDriverConfig &config); virtual ~BMI088() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual void RunImpl() = 0; @@ -74,7 +72,7 @@ class BMI088 : public device::I2C, public I2CSPIDriver int _total_failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -83,9 +81,7 @@ class BMI088 : public device::I2C, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::SELFTEST}; + } _state{STATE::SELFTEST}; uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp similarity index 97% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp index 3f213a66ddc3..6d7e4d673907 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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,18 +33,17 @@ #include "BMI088_Accelerometer.hpp" -#include // CONSTANTS_ONE_G +#include // CONSTANTS_ONE_G using namespace time_literals; namespace Bosch::BMI088::Accelerometer { -BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_accel(get_device_id(), rotation) +BMI088_Accelerometer::BMI088_Accelerometer(const I2CSPIDriverConfig &config) : + BMI088(config), + _px4_accel(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } @@ -206,22 +205,22 @@ void BMI088_Accelerometer::ConfigureAccel() switch (ACC_RANGE) { case acc_range_3g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(3.f); + _px4_accel.set_range(3.f * CONSTANTS_ONE_G); break; case acc_range_6g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(6.f); + _px4_accel.set_range(6.f * CONSTANTS_ONE_G); break; case acc_range_12g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(12.f); + _px4_accel.set_range(12.f * CONSTANTS_ONE_G); break; case acc_range_24g: _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); - _px4_accel.set_range(24.f); + _px4_accel.set_range(24.f * CONSTANTS_ONE_G); break; } } @@ -294,11 +293,7 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI088_Accelerometer::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + ScheduleNow(); } bool BMI088_Accelerometer::DataReadyInterruptConfigure() @@ -589,7 +584,7 @@ void BMI088_Accelerometer::FIFOReset() RegisterWrite(Register::ACC_SOFTRESET, 0xB0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } void BMI088_Accelerometer::UpdateTemperature() diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp similarity index 93% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp index b46bd23c51c0..189ffe9a7517 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI088::Accelerometer class BMI088_Accelerometer : public BMI088 { public: - BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI088_Accelerometer(const I2CSPIDriverConfig &config); ~BMI088_Accelerometer() override; void RunImpl() override; @@ -60,7 +59,7 @@ class BMI088_Accelerometer : public BMI088 static constexpr uint32_t RATE{1600}; // 1600 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp similarity index 95% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index 3475ff58d939..ad177c68e50b 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,12 +40,11 @@ using namespace time_literals; namespace Bosch::BMI088::Gyroscope { -BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), - _px4_gyro(get_device_id(), rotation) +BMI088_Gyroscope::BMI088_Gyroscope(const I2CSPIDriverConfig &config) : + BMI088(config), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } @@ -263,11 +262,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI088_Gyroscope::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool BMI088_Gyroscope::DataReadyInterruptConfigure() @@ -383,7 +379,7 @@ void BMI088_Gyroscope::FIFOReset() RegisterWrite(Register::FIFO_CONFIG_1, 0); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); // FIFO_CONFIG_0: restore FIFO watermark // FIFO_CONFIG_1: re-enable FIFO diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp similarity index 92% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp index 865e1069d1db..33d95b3b14a1 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -45,8 +45,7 @@ namespace Bosch::BMI088::Gyroscope class BMI088_Gyroscope : public BMI088 { public: - BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + BMI088_Gyroscope(const I2CSPIDriverConfig &config); ~BMI088_Gyroscope() override; void RunImpl() override; @@ -59,7 +58,7 @@ class BMI088_Gyroscope : public BMI088 static constexpr uint32_t RATE{400}; // 2000 Hz static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; // Transfer data struct FIFOTransferBuffer { diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp similarity index 100% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp rename to src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp similarity index 100% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp rename to src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt similarity index 100% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt rename to src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt diff --git a/src/drivers/imu/bosch/bmi088_i2c/Kconfig b/src/drivers/imu/bosch/bmi088_i2c/Kconfig new file mode 100644 index 000000000000..b4e48f2da9e8 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088_i2c/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_BOSCH_BMI088_I2C + bool "bosch bmi088_i2c" + default n + ---help--- + Enable support for bosch bmi088_i2c diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp b/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp similarity index 83% rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp rename to src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp index b7a27490e073..34f5c46f9c5c 100644 --- a/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -45,7 +45,7 @@ void BMI088::print_usage() PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true); PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x18); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -54,38 +54,41 @@ extern "C" int bmi088_i2c_main(int argc, char *argv[]) { int ch; using ThisDriver = BMI088; - BusCLIArguments cli{true, true}; + BusCLIArguments cli{true, false}; cli.i2c_address = 0x18; cli.default_i2c_frequency = 400 * 1000; - cli.default_spi_frequency = 400 * 1000; + uint16_t type = 0; + const char *name = MODULE_NAME; - while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) { switch (ch) { case 'A': - cli.type = DRV_ACC_DEVTYPE_BMI088; + type = DRV_ACC_DEVTYPE_BMI088; + name = MODULE_NAME "_accel"; cli.i2c_address = 0x18; break; case 'G': - cli.type = DRV_GYR_DEVTYPE_BMI088; + type = DRV_GYR_DEVTYPE_BMI088; + name = MODULE_NAME "_gyro"; cli.i2c_address = 0x69; break; case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); - if (!verb) { + if (!verb || type == 0) { ThisDriver::print_usage(); return -1; } - BusInstanceIterator iterator(MODULE_NAME, cli, cli.type); + BusInstanceIterator iterator(name, cli, type); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 0a7e9161ae6f..9f755f4d8d54 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -65,11 +65,10 @@ static constexpr uint8_t _checked_registers[] { using namespace time_literals; -FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, - int i2c_address) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), +FXAS21002C::FXAS21002C(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), _interface(interface), - _px4_gyro(_interface->get_device_id(), rotation), + _px4_gyro(_interface->get_device_id(), config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp index 57ba1a719a0b..0c21eb70fdd5 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp @@ -191,11 +191,10 @@ struct RawGyroReport { class FXAS21002C : public I2CSPIDriver { public: - FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int i2c_address); + FXAS21002C(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~FXAS21002C(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); diff --git a/src/drivers/imu/fxas21002c/Kconfig b/src/drivers/imu/fxas21002c/Kconfig new file mode 100644 index 000000000000..15d7c39c814f --- /dev/null +++ b/src/drivers/imu/fxas21002c/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_FXAS21002C + bool "fxas21002c" + default n + ---help--- + Enable support for fxas21002c \ No newline at end of file diff --git a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp index 854641b0667f..4d359d99da30 100644 --- a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp +++ b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019, 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 @@ -50,16 +50,15 @@ FXAS21002C::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *FXAS21002C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *FXAS21002C::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = FXAS21002C_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address); + if (config.bus_type == BOARD_I2C_BUS) { + interface = FXAS21002C_I2C_interface(config.bus, config.bus_frequency, config.i2c_address); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = FXAS21002C_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = FXAS21002C_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -67,8 +66,7 @@ I2CSPIDriverBase *FXAS21002C::instantiate(const BusCLIArguments &cli, const BusI return nullptr; } - FXAS21002C *dev = new FXAS21002C(interface, iterator.configuredBusOption(), iterator.bus(), cli.rotation, - cli.i2c_address); + FXAS21002C *dev = new FXAS21002C(interface, config); if (dev == nullptr) { delete interface; @@ -103,15 +101,15 @@ extern "C" int fxas21002c_main(int argc, char *argv[]) cli.spi_mode = SPIDEV_MODE0; cli.i2c_address = 0x20; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index ff22e5f2421f..fc5a22d214ce 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -53,13 +53,12 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = FXOS8701CQ_M_CTRL_REG2, }; -FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, - int i2c_address) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), +FXOS8701CQ::FXOS8701CQ(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), _interface(interface), - _px4_accel(interface->get_device_id(), rotation), + _px4_accel(interface->get_device_id(), config.rotation), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _px4_mag(interface->get_device_id(), rotation), + _px4_mag(interface->get_device_id(), config.rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")), #endif _accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")), diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp index c9ace08ae25b..ac3954a8e29d 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include @@ -138,11 +138,10 @@ extern device::Device *FXOS8701CQ_I2C_interface(int bus, int bus_frequency, int class FXOS8701CQ : public I2CSPIDriver { public: - FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int i2c_address); + FXOS8701CQ(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~FXOS8701CQ(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); int init(); diff --git a/src/drivers/imu/fxos8701cq/Kconfig b/src/drivers/imu/fxos8701cq/Kconfig new file mode 100644 index 000000000000..697de1dab1ed --- /dev/null +++ b/src/drivers/imu/fxos8701cq/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_FXOS8701CQ + bool "fxos8701cq" + default n + ---help--- + Enable support for fxos8701cq \ No newline at end of file diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp index 50866f8b233a..c0670242ac70 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2019, 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 @@ -56,16 +56,15 @@ FXOS8701CQ::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *FXOS8701CQ::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *FXOS8701CQ::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = FXOS8701CQ_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address); + if (config.bus_type == BOARD_I2C_BUS) { + interface = FXOS8701CQ_I2C_interface(config.bus, config.bus_frequency, config.i2c_address); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = FXOS8701CQ_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = FXOS8701CQ_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -73,8 +72,7 @@ I2CSPIDriverBase *FXOS8701CQ::instantiate(const BusCLIArguments &cli, const BusI return nullptr; } - FXOS8701CQ *dev = new FXOS8701CQ(interface, iterator.configuredBusOption(), iterator.bus(), cli.rotation, - cli.i2c_address); + FXOS8701CQ *dev = new FXOS8701CQ(interface, config); if (dev == nullptr) { delete interface; @@ -109,15 +107,15 @@ extern "C" int fxos8701cq_main(int argc, char *argv[]) cli.spi_mode = SPIDEV_MODE0; cli.i2c_address = 0x1E; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/CMakeLists.txt b/src/drivers/imu/invensense/CMakeLists.txt deleted file mode 100644 index fb82530357e9..000000000000 --- a/src/drivers/imu/invensense/CMakeLists.txt +++ /dev/null @@ -1,43 +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. -# -############################################################################ - -add_subdirectory(icm20602) -add_subdirectory(icm20608g) -add_subdirectory(icm20649) -add_subdirectory(icm20689) -add_subdirectory(icm40609d) -add_subdirectory(icm42605) -add_subdirectory(icm42688p) -add_subdirectory(mpu6000) -add_subdirectory(mpu6500) -add_subdirectory(mpu9250) diff --git a/src/drivers/imu/invensense/Kconfig b/src/drivers/imu/invensense/Kconfig new file mode 100644 index 000000000000..772bb882e8b7 --- /dev/null +++ b/src/drivers/imu/invensense/Kconfig @@ -0,0 +1,3 @@ +menu "Invensense" +rsource "*/Kconfig" +endmenu #Invensense diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt index cd14e1567459..05ed93d3078f 100644 --- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 72e63f74743a..c13ab0b800fe 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20602::ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM20602, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM20602::ICM20602(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -106,6 +105,30 @@ void ICM20602::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20602::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20602::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -140,6 +163,21 @@ void ICM20602::RunImpl() && (RegisterRead(Register::PWR_MGMT_1) == 0x41) && (RegisterRead(Register::CONFIG) == 0x80)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::XG_OFFS_TC_L); + StoreCheckedRegisterValue(Register::YG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::YG_OFFS_TC_L); + StoreCheckedRegisterValue(Register::ZG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::ZG_OFFS_TC_L); + + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Disable I2C, wakeup, and reset digital signal path RegisterWrite(Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS); // set immediately to prevent switching into I2C mode RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); @@ -199,15 +237,19 @@ void ICM20602::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -227,21 +269,34 @@ void ICM20602::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest - - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); - samples = 0; + samples = fifo_count / sizeof(FIFO::DATA); + + if (samples > _fifo_gyro_samples) { + // grab desired number of samples, but reschedule next cycle sooner + int extra_samples = samples - _fifo_gyro_samples; + samples = _fifo_gyro_samples; + + if (_fifo_gyro_samples > extra_samples) { + // reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO + const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast(FIFO_SAMPLE_DT); + ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us); + + } else { + // otherwise reschedule to run immediately + ScheduleOnInterval(_fifo_empty_interval_us); + } + + } else if (samples < _fifo_gyro_samples) { + // reschedule next cycle to catch the desired number of samples + ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast(FIFO_SAMPLE_DT)); } } } bool success = false; - if (samples >= SAMPLES_PER_TRANSFER) { - if (FIFORead(now, samples)) { + if (samples == _fifo_gyro_samples) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -397,11 +452,8 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20602::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool ICM20602::DataReadyInterruptConfigure() @@ -492,16 +544,14 @@ bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) } const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); + const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - if (fifo_count_bytes >= FIFO::SIZE) { + if ((fifo_count_bytes >= FIFO::SIZE) || (fifo_count_samples > FIFO_MAX_SAMPLES)) { perf_count(_fifo_overflow_perf); FIFOReset(); return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - if (fifo_count_samples == 0) { + } else if (fifo_count_samples == 0) { perf_count(_fifo_empty_perf); return false; } @@ -533,7 +583,7 @@ void ICM20602::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index a1a2746c470a..3855ce12ea93 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM20602; class ICM20602 : public device::SPI, public I2CSPIDriver { public: - ICM20602(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM20602(const I2CSPIDriverConfig &config); ~ICM20602() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class ICM20602 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -113,6 +110,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -142,7 +140,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -150,15 +148,13 @@ class ICM20602 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{12}; + static constexpr uint8_t size_register_cfg{24}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, @@ -173,5 +169,17 @@ class ICM20602 : public device::SPI, public I2CSPIDriver { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, { Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS, 0 }, + { Register::XG_OFFS_TC_H, 0, 0 }, + { Register::XG_OFFS_TC_L, 0, 0 }, + { Register::YG_OFFS_TC_H, 0, 0 }, + { Register::YG_OFFS_TC_L, 0, 0 }, + { Register::ZG_OFFS_TC_H, 0, 0 }, + { Register::ZG_OFFS_TC_L, 0, 0 }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index 110046544aed..682539fe33d2 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -65,6 +65,16 @@ static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C enum class Register : uint8_t { + + XG_OFFS_TC_H = 0x04, + XG_OFFS_TC_L = 0x05, + + YG_OFFS_TC_H = 0x07, + YG_OFFS_TC_L = 0x08, + + ZG_OFFS_TC_H = 0x0A, + ZG_OFFS_TC_L = 0x0B, + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -92,6 +102,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20602/Kconfig b/src/drivers/imu/invensense/icm20602/Kconfig new file mode 100644 index 000000000000..c3f3c4600feb --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM20602 + bool "icm20602" + default n + ---help--- + Enable support for icm20602 \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp index 7a48d456b6a3..28c88d0fb7ec 100644 --- a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp +++ b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,25 +46,6 @@ void ICM20602::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM20602::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM20602 *instance = new ICM20602(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm20602_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm20602_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt index df58f3200ba2..307ca6c21643 100644 --- a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index a06d54cadf6d..06fe4b048ee8 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20608G::ICM20608G(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM20608G, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM20608G::ICM20608G(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -106,6 +105,30 @@ void ICM20608G::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20608G::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20608G::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,6 +162,14 @@ void ICM20608G::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Wakeup and reset digital signal path RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); @@ -197,9 +228,16 @@ void ICM20608G::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -220,16 +258,21 @@ void ICM20608G::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -371,13 +414,10 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20608G::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -485,8 +525,8 @@ void ICM20608G::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index 5bdead39d243..9608eeb816ac 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM20608G; class ICM20608G : public device::SPI, public I2CSPIDriver { public: - ICM20608G(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM20608G(const I2CSPIDriverConfig &config); ~ICM20608G() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class ICM20608G : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -110,6 +107,7 @@ class ICM20608G : public device::SPI, public I2CSPIDriver bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -140,8 +138,8 @@ class ICM20608G : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -149,15 +147,13 @@ class ICM20608G : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{9}; + static constexpr uint8_t size_register_cfg{15}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, @@ -169,5 +165,11 @@ class ICM20608G : public device::SPI, public I2CSPIDriver { Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp index 15bd219cc243..20d901bce972 100644 --- a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp +++ b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -85,6 +85,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20608g/Kconfig b/src/drivers/imu/invensense/icm20608g/Kconfig new file mode 100644 index 000000000000..e605f40e0de1 --- /dev/null +++ b/src/drivers/imu/invensense/icm20608g/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM20608G + bool "icm20608g" + default n + ---help--- + Enable support for icm20608g diff --git a/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp b/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp index 038bf728dd88..f32d4ba620bf 100644 --- a/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp +++ b/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -46,25 +46,6 @@ void ICM20608G::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM20608G::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM20608G *instance = new ICM20608G(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm20608g_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm20608g_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index 03e40664140e..77c4012c4184 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20649::ICM20649(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM20649, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM20649::ICM20649(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -108,14 +107,27 @@ void ICM20649::print_status() int ICM20649::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM20649::RunImpl() @@ -154,8 +166,8 @@ void ICM20649::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } @@ -195,9 +207,16 @@ void ICM20649::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -218,16 +237,30 @@ void ICM20649::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + if (samples > _fifo_gyro_samples) { + // grab desired number of samples, but reschedule next cycle sooner + int extra_samples = samples - _fifo_gyro_samples; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); + if (_fifo_gyro_samples > extra_samples) { + // reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO + const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast(FIFO_SAMPLE_DT); + ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else { + // otherwise reschedule to run immediately + ScheduleOnInterval(_fifo_empty_interval_us); + } + + } else if (samples < _fifo_gyro_samples) { + // reschedule next cycle to catch the desired number of samples + ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast(FIFO_SAMPLE_DT)); + } + + if (samples == _fifo_gyro_samples) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -342,9 +375,9 @@ void ICM20649::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); } -void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); @@ -396,13 +429,10 @@ int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20649::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -488,10 +518,11 @@ void ICM20649::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits uint16_t ICM20649::FIFOReadCount() { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -503,9 +534,10 @@ uint16_t ICM20649::FIFOReadCount() bool ICM20649::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE); - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -550,8 +582,8 @@ void ICM20649::FIFOReset() RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 95e5b8ed0639..531f0be988ef 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM20649; class ICM20649 : public device::SPI, public I2CSPIDriver { public: - ICM20649(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM20649(const I2CSPIDriverConfig &config); ~ICM20649() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class ICM20649 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -112,7 +109,7 @@ class ICM20649 : public device::SPI, public I2CSPIDriver void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } @@ -155,8 +152,8 @@ class ICM20649 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -164,12 +161,10 @@ class ICM20649 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{6}; diff --git a/src/drivers/imu/invensense/icm20649/Kconfig b/src/drivers/imu/invensense/icm20649/Kconfig new file mode 100644 index 000000000000..d00418f23414 --- /dev/null +++ b/src/drivers/imu/invensense/icm20649/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM20649 + bool "icm20649" + default n + ---help--- + Enable support for icm20649 \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm20649/icm20649_main.cpp b/src/drivers/imu/invensense/icm20649/icm20649_main.cpp index ce91fffec04a..d46a59e2912f 100644 --- a/src/drivers/imu/invensense/icm20649/icm20649_main.cpp +++ b/src/drivers/imu/invensense/icm20649/icm20649_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void ICM20649::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM20649::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM20649 *instance = new ICM20649(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm20649_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm20649_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm20689/CMakeLists.txt b/src/drivers/imu/invensense/icm20689/CMakeLists.txt index e1ad8fcf2665..34ba239bc095 100644 --- a/src/drivers/imu/invensense/icm20689/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20689/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index c25127013332..8337ba2cc466 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20689::ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM20689, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM20689::ICM20689(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -106,6 +105,30 @@ void ICM20689::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20689::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20689::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,6 +162,14 @@ void ICM20689::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Wakeup and reset digital signal path RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); @@ -197,9 +228,16 @@ void ICM20689::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -220,16 +258,21 @@ void ICM20689::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -371,13 +414,10 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20689::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -485,8 +525,8 @@ void ICM20689::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index fec7fe3c6ed9..df3b8a734e90 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM20689; class ICM20689 : public device::SPI, public I2CSPIDriver { public: - ICM20689(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM20689(const I2CSPIDriverConfig &config); ~ICM20689() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class ICM20689 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -110,6 +107,7 @@ class ICM20689 : public device::SPI, public I2CSPIDriver bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -140,8 +138,8 @@ class ICM20689 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -149,15 +147,13 @@ class ICM20689 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{9}; + static constexpr uint8_t size_register_cfg{15}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, @@ -169,5 +165,11 @@ class ICM20689 : public device::SPI, public I2CSPIDriver { Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp index e577ef4e2a13..11999e9aeb97 100644 --- a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp +++ b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -85,6 +85,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20689/Kconfig b/src/drivers/imu/invensense/icm20689/Kconfig new file mode 100644 index 000000000000..0b2ab48ba993 --- /dev/null +++ b/src/drivers/imu/invensense/icm20689/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM20689 + bool "icm20689" + default n + ---help--- + Enable support for icm20689 \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm20689/icm20689_main.cpp b/src/drivers/imu/invensense/icm20689/icm20689_main.cpp index 1e0a3515239c..bd2d31527f71 100644 --- a/src/drivers/imu/invensense/icm20689/icm20689_main.cpp +++ b/src/drivers/imu/invensense/icm20689/icm20689_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,25 +46,6 @@ void ICM20689::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM20689::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM20689 *instance = new ICM20689(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm20689_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm20689_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt index 3d4965bed69c..b9ea57f5c02f 100644 --- a/src/drivers/imu/invensense/icm20948/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ px4_add_module( - MODULE drivers__imu__icm20948 + MODULE drivers__imu__invensense__icm20948 MAIN icm20948 COMPILE_FLAGS SRCS @@ -48,3 +48,16 @@ px4_add_module( drivers_gyroscope drivers_magnetometer ) + +px4_add_module( + MODULE drivers__imu__invensense__icm20948_i2c_passthrough + MAIN icm20948_i2c_passthrough + COMPILE_FLAGS + SRCS + ICM20948_I2C_Passthrough.cpp + ICM20948_I2C_Passthrough.hpp + icm20948_i2c_passthrough_main.cpp + InvenSense_ICM20948_registers.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 3bf358f6868f..2031e62b89f7 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -42,22 +42,23 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer) : - SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM20948::ICM20948(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + bool enable_magnetometer = config.custom1 == 1; + if (enable_magnetometer) { - _slave_ak09916_magnetometer = new AKM_AK09916::ICM20948_AK09916(*this, rotation); + _slave_ak09916_magnetometer = new AKM_AK09916::ICM20948_AK09916(*this, config.rotation); if (_slave_ak09916_magnetometer) { for (auto &r : _register_bank3_cfg) { @@ -135,14 +136,27 @@ void ICM20948::print_status() int ICM20948::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM20948::RunImpl() @@ -182,8 +196,8 @@ void ICM20948::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } @@ -229,9 +243,16 @@ void ICM20948::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -252,16 +273,30 @@ void ICM20948::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + if (samples > _fifo_gyro_samples) { + // grab desired number of samples, but reschedule next cycle sooner + int extra_samples = samples - _fifo_gyro_samples; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); + if (_fifo_gyro_samples > extra_samples) { + // reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO + const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast(FIFO_SAMPLE_DT); + ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else { + // otherwise reschedule to run immediately + ScheduleOnInterval(_fifo_empty_interval_us); + } + + } else if (samples < _fifo_gyro_samples) { + // reschedule next cycle to catch the desired number of samples + ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast(FIFO_SAMPLE_DT)); + } + + if (samples == _fifo_gyro_samples) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -378,9 +413,9 @@ void ICM20948::ConfigureSampleRate(int sample_rate) _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); } -void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); @@ -441,13 +476,10 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM20948::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -533,10 +565,11 @@ void ICM20948::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits uint16_t ICM20948::FIFOReadCount() { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -548,9 +581,10 @@ uint16_t ICM20948::FIFOReadCount() bool ICM20948::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE); - SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -595,8 +629,8 @@ void ICM20948::FIFOReset() RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); } static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 2dca68c67157..103a03fe573e 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -58,12 +58,9 @@ using namespace InvenSense_ICM20948; class ICM20948 : public device::SPI, public I2CSPIDriver { public: - ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer = false); + ICM20948(const I2CSPIDriverConfig &config); ~ICM20948() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -76,12 +73,12 @@ class ICM20948 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -120,7 +117,7 @@ class ICM20948 : public device::SPI, public I2CSPIDriver void ConfigureGyro(); void ConfigureSampleRate(int sample_rate); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } void SelectRegisterBank(Register::BANK_3 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_3); } @@ -172,8 +169,8 @@ class ICM20948 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -181,12 +178,10 @@ class ICM20948 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{6}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 8abe78bfa788..7e079d275d4b 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -51,7 +51,6 @@ ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) : _px4_mag(icm20948.get_device_id(), rotation) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); - _px4_mag.set_external(icm20948.external()); // mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB) _px4_mag.set_scale(1.5e-3f); diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp new file mode 100644 index 000000000000..9146a136941a --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp @@ -0,0 +1,302 @@ +/**************************************************************************** + * + * 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 "ICM20948_I2C_Passthrough.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM20948_I2C_Passthrough::ICM20948_I2C_Passthrough(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ICM20948_I2C_Passthrough::~ICM20948_I2C_Passthrough() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int ICM20948_I2C_Passthrough::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ICM20948_I2C_Passthrough::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ICM20948_I2C_Passthrough::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("temperature: %.1f degC", (double)_temperature); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int ICM20948_I2C_Passthrough::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } + } + + return PX4_ERROR; +} + +void ICM20948_I2C_Passthrough::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); + break; + + case STATE::WAIT_FOR_RESET: + + // The reset value is 0x00 for all registers other than the registers below + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) { + // Wakeup and reset + RegisterWrite(Register::BANK_0::USER_CTRL, USER_CTRL_BIT::SRAM_RST | USER_CTRL_BIT::I2C_MST_RST); + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(1_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + _state = STATE::READ; + ScheduleOnInterval(500_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(10_ms); + } + + break; + + case STATE::READ: { + if (hrt_elapsed_time(&_last_config_check_timestamp) > 1000_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) +{ + if (bank != _last_register_bank || force) { + // select BANK_0 + uint8_t cmd_bank_sel[2]; + cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); + cmd_bank_sel[1] = bank; + + transfer(cmd_bank_sel, 2, nullptr, 0); + + _last_register_bank = bank; + } +} + +bool ICM20948_I2C_Passthrough::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_bank0_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + return success; +} + +template +bool ICM20948_I2C_Passthrough::RegisterCheck(const T ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +template +uint8_t ICM20948_I2C_Passthrough::RegisterRead(T reg) +{ + SelectRegisterBank(reg); + + uint8_t cmd = static_cast(reg); + uint8_t value = 0; + transfer(&cmd, 1, &value, 1); + return value; +} + +template +void ICM20948_I2C_Passthrough::RegisterWrite(T reg, uint8_t value) +{ + SelectRegisterBank(reg); + + uint8_t cmd[2]; + cmd[0] = static_cast(reg); + cmd[1] = value; + transfer(cmd, sizeof(cmd), nullptr, 0); +} + +template +void ICM20948_I2C_Passthrough::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +void ICM20948_I2C_Passthrough::UpdateTemperature() +{ + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); + + // read current temperature + uint8_t cmd = static_cast(Register::BANK_0::TEMP_OUT_H); + uint8_t temperature_buf[2] {}; + + if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]); + const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _temperature = TEMP_degC; + } +} diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp new file mode 100644 index 000000000000..cc8fa12358cf --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 ICM20948_I2C_Passthrough.hpp + * + * Driver for the Invensense ICM20948 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM20948_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace InvenSense_ICM20948; + +class ICM20948_I2C_Passthrough : public device::I2C, public I2CSPIDriver +{ +public: + ICM20948_I2C_Passthrough(const I2CSPIDriverConfig &config); + ~ICM20948_I2C_Passthrough() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } + + template bool RegisterCheck(const T ®_cfg); + + template uint8_t RegisterRead(T reg); + template void RegisterWrite(T reg, uint8_t value); + template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); + + void UpdateTemperature(); + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + float _temperature{NAN}; + + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{3}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::USER_CTRL, 0, USER_CTRL_BIT::DMP_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS }, + { Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::BYPASS_EN, 0 }, + }; +}; diff --git a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp index ae0c81cb6e7b..12fcbdbd626c 100644 --- a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp +++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp @@ -54,6 +54,9 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x69; // 0b1101001 + static constexpr uint32_t SPI_SPEED = 7 * 1000 * 1000; // 7 MHz SPI static constexpr uint8_t DIR_READ = 0x80; @@ -97,8 +100,11 @@ enum class BANK_0 : uint8_t { }; enum class BANK_2 : uint8_t { + GYRO_SMPLRT_DIV = 0x00, GYRO_CONFIG_1 = 0x01, + ACCEL_SMPLRT_DIV_2 = 0x11, + ACCEL_CONFIG = 0x14, REG_BANK_SEL = 0x7F, @@ -146,6 +152,7 @@ enum PWR_MGMT_1_BIT : uint8_t { // INT_PIN_CFG enum INT_PIN_CFG_BIT : uint8_t { INT1_ACTL = Bit7, + BYPASS_EN = Bit1, // When asserted, the I2C_MASTER interface pins (ES_CL and ES_DA) will go into ‘bypass mode’ when the I 2 C master interface is disabled. }; // INT_ENABLE_1 @@ -188,7 +195,10 @@ enum REG_BANK_SEL_BIT : uint8_t { //---------------- BANK2 Register bits // GYRO_CONFIG_1 enum GYRO_CONFIG_1_BIT : uint8_t { - // GYRO_FS_SEL[1:0] + // 5:3 GYRO_DLPFCFG[2:0] + GYRO_DLPFCFG = Bit5 | Bit4 | Bit3, // 7 - + + // 2:1 GYRO_FS_SEL[1:0] GYRO_FS_SEL_250_DPS = 0, // 0b00 = ±250 dps GYRO_FS_SEL_500_DPS = Bit1, // 0b01 = ±500 dps GYRO_FS_SEL_1000_DPS = Bit2, // 0b10 = ±1000 dps @@ -199,7 +209,10 @@ enum GYRO_CONFIG_1_BIT : uint8_t { // ACCEL_CONFIG enum ACCEL_CONFIG_BIT : uint8_t { - // ACCEL_FS_SEL[1:0] + // 5:3 ACCEL_DLPFCFG[2:0] + ACCEL_DLPFCFG = Bit5 | Bit4 | Bit3, // 7 - + + // 2:1 ACCEL_FS_SEL[1:0] ACCEL_FS_SEL_2G = 0, // 0b00: ±2g ACCEL_FS_SEL_4G = Bit1, // 0b01: ±4g ACCEL_FS_SEL_8G = Bit2, // 0b10: ±8g diff --git a/src/drivers/imu/invensense/icm20948/Kconfig b/src/drivers/imu/invensense/icm20948/Kconfig new file mode 100644 index 000000000000..42ac914cbfdf --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM20948 + bool "icm20948" + default n + ---help--- + Enable support for icm20948 \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp b/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp new file mode 100644 index 000000000000..ea98288acddd --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/icm20948_i2c_passthrough_main.cpp @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 "ICM20948_I2C_Passthrough.hpp" + +#include +#include + +void ICM20948_I2C_Passthrough::print_usage() +{ + PRINT_MODULE_USAGE_NAME("icm20948_i2c_passthrough", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x69); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int icm20948_i2c_passthrough_main(int argc, char *argv[]) +{ + using ThisDriver = ICM20948_I2C_Passthrough; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948); + + 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/imu/invensense/icm20948/icm20948_main.cpp b/src/drivers/imu/invensense/icm20948/icm20948_main.cpp index c81ecfc08773..bab15cb3cd50 100644 --- a/src/drivers/imu/invensense/icm20948/icm20948_main.cpp +++ b/src/drivers/imu/invensense/icm20948/icm20948_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -47,26 +47,6 @@ void ICM20948::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - bool mag = (cli.custom1 == 1); - ICM20948 *instance = new ICM20948(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO(), mag); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm20948_main(int argc, char *argv[]) { int ch; @@ -74,19 +54,19 @@ extern "C" int icm20948_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "MR:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "MR:")) != EOF) { switch (ch) { case 'M': cli.custom1 = 1; break; case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 881be246f9cd..8dc0f4b79512 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM40609D::ICM40609D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM40609D, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM40609D::ICM40609D(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -108,14 +107,27 @@ void ICM40609D::print_status() int ICM40609D::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM40609D::RunImpl() @@ -190,15 +202,19 @@ void ICM40609D::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -220,6 +236,12 @@ void ICM40609D::RunImpl() // FIFO count (size in bytes) samples = (fifo_count / sizeof(FIFO::DATA)); + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); @@ -232,7 +254,7 @@ void ICM40609D::RunImpl() bool success = false; if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -367,9 +389,9 @@ void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); @@ -410,11 +432,8 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM40609D::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool ICM40609D::DataReadyInterruptConfigure() @@ -581,7 +600,7 @@ void ICM40609D::FIFOReset() RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 1b7578246a2c..ab6fcfa1f27b 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM40609D; class ICM40609D : public device::SPI, public I2CSPIDriver { public: - ICM40609D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM40609D(const I2CSPIDriverConfig &config); ~ICM40609D() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -78,7 +75,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -107,7 +104,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } static int DataReadyInterruptCallback(int irq, void *context, void *arg); @@ -149,7 +146,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -157,12 +154,10 @@ class ICM40609D : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{10}; diff --git a/src/drivers/imu/invensense/icm40609d/Kconfig b/src/drivers/imu/invensense/icm40609d/Kconfig new file mode 100644 index 000000000000..0981df7fbdab --- /dev/null +++ b/src/drivers/imu/invensense/icm40609d/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM40609D + bool "icm40609d" + default n + ---help--- + Enable support for icm40609d \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp b/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp index a366698fc031..92480f05995f 100644 --- a/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp +++ b/src/drivers/imu/invensense/icm40609d/icm40609d_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void ICM40609D::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM40609D::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM40609D *instance = new ICM40609D(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm40609d_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm40609d_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 60827cd0a755..e805382194bd 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM42605::ICM42605(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM42605, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM42605::ICM42605(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -108,14 +107,27 @@ void ICM42605::print_status() int ICM42605::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM42605::RunImpl() @@ -191,15 +203,19 @@ void ICM42605::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -221,6 +237,12 @@ void ICM42605::RunImpl() // FIFO count (size in bytes) samples = (fifo_count / sizeof(FIFO::DATA)); + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); @@ -233,7 +255,7 @@ void ICM42605::RunImpl() bool success = false; if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -368,9 +390,9 @@ void ICM42605::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); @@ -411,11 +433,8 @@ int ICM42605::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42605::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool ICM42605::DataReadyInterruptConfigure() @@ -582,7 +601,7 @@ void ICM42605::FIFOReset() RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } void ICM42605::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index bc3a4670ced6..98ad916d90dc 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM42605; class ICM42605 : public device::SPI, public I2CSPIDriver { public: - ICM42605(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM42605(const I2CSPIDriverConfig &config); ~ICM42605() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -78,7 +75,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -107,7 +104,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } static int DataReadyInterruptCallback(int irq, void *context, void *arg); @@ -149,7 +146,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -157,12 +154,10 @@ class ICM42605 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{10}; diff --git a/src/drivers/imu/invensense/icm42605/Kconfig b/src/drivers/imu/invensense/icm42605/Kconfig new file mode 100644 index 000000000000..a280b6da6c69 --- /dev/null +++ b/src/drivers/imu/invensense/icm42605/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM42605 + bool "icm42605" + default n + ---help--- + Enable support for icm42605 \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm42605/icm42605_main.cpp b/src/drivers/imu/invensense/icm42605/icm42605_main.cpp index f8c870ab1bbc..4dd6fa2e3792 100644 --- a/src/drivers/imu/invensense/icm42605/icm42605_main.cpp +++ b/src/drivers/imu/invensense/icm42605/icm42605_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void ICM42605::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM42605::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM42605 *instance = new ICM42605(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm42605_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm42605_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/icm42670p/CMakeLists.txt b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt new file mode 100644 index 000000000000..b426216c306a --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2021-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. +# +############################################################################ +px4_add_module( + MODULE drivers__imu__invensense__icm42670p + MAIN icm42670p + COMPILE_FLAGS + SRCS + icm42670p_main.cpp + ICM42670P.cpp + ICM42670P.hpp + InvenSense_ICM42670P_registers.hpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + ) diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp new file mode 100644 index 000000000000..a7d1244a43c5 --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp @@ -0,0 +1,663 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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 "ICM42670P.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM42670P::ICM42670P(const I2CSPIDriverConfig &config): + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +ICM42670P::~ICM42670P() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +int ICM42670P::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ICM42670P::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ICM42670P::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void ICM42670P::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int ICM42670P::probe() +{ + const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void ICM42670P::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // SIGNAL_PATH_RESET: Software Reset (auto clear bit) + RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00) + && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { + + // Wakeup accel and gyro and schedule remaining configuration + RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE); + _state = STATE::CONFIGURE; + ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + if (samples == 0) { + // check current FIFO count + const uint16_t fifo_count = FIFOReadCount(); + + if (fifo_count >= FIFO::SIZE) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_count == 0) { + perf_count(_fifo_empty_perf); + + } else { + // FIFO count (size in bytes) + samples = (fifo_count / sizeof(FIFO::DATA)); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + samples = 0; + } + } + } + + bool success = false; + + if (samples >= 1) { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) + && RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1]) + ) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void ICM42670P::ConfigureSampleRate(int sample_rate) +{ + // round down to nearest FIFO sample dt + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +void ICM42670P::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO watermark threshold in number of bytes + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); + + for (auto &r : _register_bank0_cfg) { + if (r.reg == Register::BANK_0::FIFO_CONFIG2) { + // FIFO_WM[7:0] FIFO_CONFIG2 + r.set_bits = fifo_watermark_threshold & 0xFF; + + } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { + // FIFO_WM[11:8] FIFO_CONFIG3 + r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; + } + } +} + +bool ICM42670P::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_bank0_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_mreg1_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + for (const auto ®_cfg : _register_mreg1_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + + _px4_gyro.set_scale(math::radians(2000.f / 32768.f)); + _px4_gyro.set_range(math::radians(2000.f)); + + return success; +} + +int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ICM42670P::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool ICM42670P::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool ICM42670P::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +template +bool ICM42670P::RegisterCheck(const T ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +uint8_t ICM42670P::RegisterRead(Register::MREG1 reg) +{ + // BLK_SEL_R must be set to 0 + RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00); + + // MADDR_R must be set to 0x14 (address of the MREG1 register being accessed) + RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg); + + // Wait for 10 µs + px4_udelay(10); + + // Read register M_R to access the value in MREG1 register 0x14 + uint8_t value = RegisterRead(Register::BANK_0::M_R); + + // Wait for 10 µs + // Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off. + px4_udelay(10); + + return value; +} + +void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value) +{ + // BLK_SEL_W must be set to 0 + RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00); + + // MADDR_W must be set to address of the MREG1 register being accessed + RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg); + + // M_W must be set to the desired value + RegisterWrite(Register::BANK_0::M_W, value); + + // Wait for 10 µs + // Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off. + px4_udelay(10); +} + + +template +void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t ICM42670P::FIFOReadCount() +{ + // read FIFO count + uint8_t fifo_count_buf[3] {}; + fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; + + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool ICM42670P::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + return false; + } + + const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); + + if (fifo_count_bytes >= FIFO::SIZE) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + return false; + } + + const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); + + if (fifo_count_samples == 0) { + perf_count(_fifo_empty_perf); + return false; + } + + // check FIFO header in every sample + uint8_t valid_samples = 0; + + for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + bool valid = true; + + // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx + const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header; + + if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) { + // FIFO sample empty if HEADER_MSG set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) { + // accel bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { + // gyro bit not set + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) { + // Packet does not contain a new and valid extended 20-bit data + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { + // accel ODR changed + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) { + // gyro ODR changed + valid = false; + } + + if (valid) { + valid_samples++; + + } else { + perf_count(_bad_transfer_perf); + break; + } + } + + if (valid_samples > 0) { + ProcessGyro(timestamp_sample, buffer.f, valid_samples); + ProcessAccel(timestamp_sample, buffer.f, valid_samples); + return true; + } + + return false; +} + +void ICM42670P::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO flush requires the following programming sequence: + // Write FIFO_FLUSH = 1 + // Wait for 1.5 µs + // Read FIFO_FLUSH, it should now be 0 + + // SIGNAL_PATH_RESET: FIFO flush + RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + px4_udelay(2); // Wait for 1.5 µs + + const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET); + + if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) { + PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed"); + } + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); +} + +void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = samples; + accel.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0); + int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0); + int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[i] = accel_x; + accel.y[i] = math::negate(accel_y); + accel.z[i] = math::negate(accel_z); + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + _px4_accel.updateFIFO(accel); +} + +void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + const int16_t gyro_x = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); + const int16_t gyro_y = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0); + const int16_t gyro_z = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); + } + + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + _px4_gyro.updateFIFO(gyro); +} + +void ICM42670P::UpdateTemperature() +{ + // read current temperature + uint8_t temperature_buf[3] {}; + temperature_buf[0] = static_cast(Register::BANK_0::TEMP_DATA1) | DIR_READ; + + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]); + + // Temperature in Degrees Centigrade + const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + } +} diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp new file mode 100644 index 000000000000..362418f7aea3 --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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 ICM42688P.hpp + * + * Driver for the Invensense ICM42688P connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM42670P_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_ICM42670P; + +class ICM42670P : public device::SPI, public I2CSPIDriver +{ +public: + ICM42670P(const I2CSPIDriverConfig &config); + ~ICM42670P() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f}; // 1600 Hz accel & gyro ODR configured + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; + static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; + + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::BANK_0::INT_STATUS) | DIR_READ}; + uint8_t INT_STATUS{0}; + uint8_t INT_STATUS2{0}; + uint8_t INT_STATUS3{0}; + uint8_t FIFO_COUNTH{0}; + uint8_t FIFO_COUNTL{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_mreg1_config_t { + Register::MREG1 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + void ConfigureSampleRate(int sample_rate); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + uint8_t RegisterRead(Register::BANK_0 reg); + uint8_t RegisterRead(Register::MREG1 reg); + + void RegisterWrite(Register::BANK_0 reg, uint8_t value); + void RegisterWrite(Register::MREG1 reg, uint8_t value); + + template bool RegisterCheck(const T ®_cfg); + template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); + template void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + template void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; + perf_counter_t _drdy_missed_perf{nullptr}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{10}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, + { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR }, + { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS }, + { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime + { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime + { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + }; + + uint8_t _checked_register_mreg1{0}; + static constexpr uint8_t size_register_mreg1_cfg{2}; + register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] { + // Register | Set bits, Clear bits + { Register::MREG1::FIFO_CONFIG5, FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 }, + { Register::MREG1::INT_CONFIG0, INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 }, + }; +}; diff --git a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp new file mode 100644 index 000000000000..5e9d8259c73f --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp @@ -0,0 +1,241 @@ +/**************************************************************************** + * + * Copyright (c) 2021-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 InvenSense_ICM42670P_registers.hpp + * + * Invensense ICM-42670-P registers. + * + */ + +#pragma once + +#include + +namespace InvenSense_ICM42670P +{ +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0x67; + +static constexpr float TEMPERATURE_SENSITIVITY = 128.0f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 25.f; // C + +namespace Register +{ + +enum class BANK_0 : uint8_t { + SIGNAL_PATH_RESET = 0x02, + + INT_CONFIG = 0x06, + + TEMP_DATA1 = 0x09, + TEMP_DATA0 = 0x0A, + + PWR_MGMT0 = 0x1F, + GYRO_CONFIG0 = 0x20, + ACCEL_CONFIG0 = 0x21, + GYRO_CONFIG1 = 0x23, + ACCEL_CONFIG1 = 0x24, + + FIFO_CONFIG1 = 0x28, + FIFO_CONFIG2 = 0x29, + FIFO_CONFIG3 = 0x2A, + INT_SOURCE0 = 0x2B, + + INT_STATUS = 0x3A, + FIFO_COUNTH = 0x3D, + FIFO_COUNTL = 0x3E, + FIFO_DATA = 0x3F, + + WHO_AM_I = 0x75, + + BLK_SEL_W = 0x79, + MADDR_W = 0x7A, + M_W = 0x7B, + BLK_SEL_R = 0x7C, + MADDR_R = 0x7D, + M_R = 0x7E, +}; + +enum class MREG1 : uint8_t { + FIFO_CONFIG5 = 0x01, + + INT_CONFIG0 = 0x04, +}; + +}; + +//---------------- BANK0 Register bits + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + SOFT_RESET_DEVICE_CONFIG = Bit4, // 1: Software reset enabled + FIFO_FLUSH = Bit2, // When set to 1, FIFO will get flushed +}; + +// INT_CONFIG +enum INT_CONFIG_BIT : uint8_t { + INT1_MODE = Bit2, // INT1 interrupt mode 1: Latched mode + INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull + INT1_POLARITY = Bit0, // INT1 interrupt polarity 0: Active low +}; + +// PWR_MGMT0 +enum PWR_MGMT0_BIT : uint8_t { + // 3:2 GYRO_MODE + GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode + // 1:0 ACCEL_MODE + ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode +}; + +// GYRO_CONFIG0 +enum GYRO_CONFIG0_BIT : uint8_t { + // 6:5 GYRO_FS_SEL + GYRO_FS_SEL_2000_DPS_SET = 0, // 0b000 = ±2000dps + GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5, + + // 3:0 GYRO_ODR + GYRO_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz + GYRO_ODR_1600HZ_CLEAR = Bit3 | Bit1, +}; + +// ACCEL_CONFIG0 +enum ACCEL_CONFIG0_BIT : uint8_t { + // 6:5 ACCEL_UI_FS_SEL + ACCEL_UI_FS_SEL_16G_SET = 0, // 000: ±16g + ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5, + + // 3:0 ACCEL_ODR + ACCEL_ODR_1600HZ_SET = Bit2 | Bit0, // 0101: 1600Hz + ACCEL_ODR_1600HZ_CLEAR = Bit3 | Bit1, +}; + +// GYRO_CONFIG1 +enum GYRO_CONFIG1_BIT : uint8_t { + // 2:0 GYRO_UI_FILT_BW + GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed +}; + +// ACCEL_CONFIG1 +enum ACCEL_CONFIG1_BIT : uint8_t { + // 2:0 ACCEL_UI_FILT_BW + ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed +}; + +// FIFO_CONFIG1 +enum FIFO_CONFIG1_BIT : uint8_t { + // FIFO_MODE + FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode + FIFO_BYPASS = Bit0, // 0: FIFO is not bypassed +}; + +// INT_STATUS +enum INT_STATUS_BIT : uint8_t { + RESET_DONE_INT = Bit4, + FIFO_THS_INT = Bit2, + FIFO_FULL_INT = Bit1, +}; + +// INT_SOURCE0 +enum INT_SOURCE0_BIT : uint8_t { + RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1 + FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 +}; + + +//---------------- USER BANK MREG1 Register bits + +// FIFO_CONFIG5 +enum FIFO_CONFIG5_BIT : uint8_t { + FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM + FIFO_GYRO_EN = Bit1, // 1: Enables Gyro packets to go to FIFO + FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO +}; + +// INT_CONFIG0 +enum INT_CONFIG0_BIT : uint8_t { + // 3:2 FIFO_THS_INT_CLEAR + FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read +}; + +namespace FIFO +{ +static constexpr size_t SIZE = 1024; + +// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set + +// Packet 3 +struct DATA { + uint8_t FIFO_Header; + uint8_t ACCEL_DATA_X1; // Accel X [19:12] + uint8_t ACCEL_DATA_X0; // Accel X [11:4] + uint8_t ACCEL_DATA_Y1; // Accel Y [19:12] + uint8_t ACCEL_DATA_Y0; // Accel Y [11:4] + uint8_t ACCEL_DATA_Z1; // Accel Z [19:12] + uint8_t ACCEL_DATA_Z0; // Accel Z [11:4] + uint8_t GYRO_DATA_X1; // Gyro X [19:12] + uint8_t GYRO_DATA_X0; // Gyro X [11:4] + uint8_t GYRO_DATA_Y1; // Gyro Y [19:12] + uint8_t GYRO_DATA_Y0; // Gyro Y [11:4] + uint8_t GYRO_DATA_Z1; // Gyro Z [19:12] + uint8_t GYRO_DATA_Z0; // Gyro Z [11:4] + uint8_t temperature; // Temperature[7:0] + uint8_t TimeStamp_h; // TimeStamp[15:8] + uint8_t TimeStamp_l; // TimeStamp[7:0] +}; + +// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx +enum FIFO_HEADER_BIT : uint8_t { + HEADER_MSG = Bit7, // 1: FIFO is empty + HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 + HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 + HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, + HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet + HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet +}; + +} +} // namespace InvenSense_ICM42670P diff --git a/src/drivers/imu/invensense/icm42670p/Kconfig b/src/drivers/imu/invensense/icm42670p/Kconfig new file mode 100644 index 000000000000..2d6591da314a --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM42670P + bool "icm42670p" + default n + ---help--- + Enable support for icm42670p diff --git a/src/drivers/imu/invensense/icm42670p/icm42670p_main.cpp b/src/drivers/imu/invensense/icm42670p/icm42670p_main.cpp new file mode 100644 index 000000000000..ce171fb9364a --- /dev/null +++ b/src/drivers/imu/invensense/icm42670p/icm42670p_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 "ICM42670P.hpp" + +#include +#include + +void ICM42670P::print_usage() +{ + PRINT_MODULE_USAGE_NAME("icm42670p", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + + +extern "C" int icm42670p_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = ICM42670P; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42670P); + + 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/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index e2f22c046d5e..8f149bdf5dae 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -ICM42688P::ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -108,14 +107,27 @@ void ICM42688P::print_status() int ICM42688P::probe() { - const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true); + } + } } - return PX4_OK; + return PX4_ERROR; } void ICM42688P::RunImpl() @@ -191,15 +203,19 @@ void ICM42688P::RunImpl() break; case STATE::FIFO_READ: { - uint32_t samples = 0; + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - } else { + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -221,6 +237,12 @@ void ICM42688P::RunImpl() // FIFO count (size in bytes) samples = (fifo_count / sizeof(FIFO::DATA)); + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); @@ -233,7 +255,7 @@ void ICM42688P::RunImpl() bool success = false; if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -252,20 +274,22 @@ void ICM42688P::RunImpl() } } - // check configuration registers periodically or immediately following any failure - if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) - && RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) - && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) - ) { - _last_config_check_timestamp = now; - _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; - _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; - _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) + && RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) + && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) + ) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; + _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; - } else { - // register check failed, force reset - perf_count(_bad_register_perf); - Reset(); + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } } } @@ -304,9 +328,9 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples) } } -void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) { - if (bank != _last_register_bank) { + if (bank != _last_register_bank || force) { // select BANK_0 uint8_t cmd_bank_sel[2] {}; cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); @@ -369,11 +393,8 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) void ICM42688P::DataReady() { - uint32_t expected = 0; - - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - ScheduleNow(); - } + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); } bool ICM42688P::DataReadyInterruptConfigure() @@ -554,7 +575,7 @@ void ICM42688P::FIFOReset() RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); // reset while FIFO is disabled - _drdy_fifo_read_samples.store(0); + _drdy_timestamp_sample.store(0); } static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c) diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index c8e9aa51d32f..96f1d1d8f068 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_ICM42688P; class ICM42688P : public device::SPI, public I2CSPIDriver { public: - ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ICM42688P(const I2CSPIDriverConfig &config); ~ICM42688P() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -73,12 +70,12 @@ class ICM42688P : public device::SPI, public I2CSPIDriver void exit_and_cleanup() override; // Sensor Configuration - static constexpr float FIFO_SAMPLE_DT{1e6f / 16000.f}; // 16000 Hz accel & gyro ODR configured + static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -117,7 +114,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver void ConfigureSampleRate(int sample_rate); void ConfigureFIFOWatermark(uint8_t samples); - void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); } void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } @@ -161,7 +158,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -172,7 +169,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; static constexpr uint8_t size_register_bank0_cfg{13}; @@ -181,8 +178,8 @@ class ICM42688P : public device::SPI, public I2CSPIDriver { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, - { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_16kHz, Bit3 | Bit2 | Bit0 }, - { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_16kHz, Bit3 | Bit2 | Bit0 }, + { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR }, { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD }, { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW }, { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD }, @@ -197,13 +194,13 @@ class ICM42688P : public device::SPI, public I2CSPIDriver static constexpr uint8_t size_register_bank1_cfg{1}; register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { // Register | Set bits, Clear bits - { Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS }, + { Register::BANK_1::GYRO_CONFIG_STATIC2, GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 }, }; uint8_t _checked_register_bank2{0}; static constexpr uint8_t size_register_bank2_cfg{1}; register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { // Register | Set bits, Clear bits - { Register::BANK_2::ACCEL_CONFIG_STATIC2, 0, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, + { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 29c6632144da..bf44666a01f2 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -159,29 +159,50 @@ enum PWR_MGMT0_BIT : uint8_t { // GYRO_CONFIG0 enum GYRO_CONFIG0_BIT : uint8_t { // 7:5 GYRO_FS_SEL - GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) + GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default) + GYRO_FS_SEL_1000_DPS = Bit5, + GYRO_FS_SEL_500_DPS = Bit6, + GYRO_FS_SEL_250_DPS = Bit6 | Bit5, + GYRO_FS_SEL_125_DPS = Bit7, + // 3:0 GYRO_ODR - GYRO_ODR_32kHz = Bit0, // 0001: 32kHz - GYRO_ODR_16kHz = Bit1, // 0010: 16kHz - GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz - GYRO_ODR_4kHz = Bit2, // 0100: 4kHz - GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz - GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) + // 0001: 32kHz + GYRO_ODR_32KHZ_SET = Bit0, + GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + GYRO_ODR_16KHZ_SET = Bit1, + GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + GYRO_ODR_8KHZ_SET = Bit1 | Bit0, + GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + GYRO_ODR_1KHZ_SET = Bit2 | Bit1, + GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0, }; // ACCEL_CONFIG0 enum ACCEL_CONFIG0_BIT : uint8_t { // 7:5 ACCEL_FS_SEL - ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) + ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) + ACCEL_FS_SEL_8G = Bit5, + ACCEL_FS_SEL_4G = Bit6, + ACCEL_FS_SEL_2G = Bit6 | Bit5, + // 3:0 ACCEL_ODR - ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz - ACCEL_ODR_16kHz = Bit1, // 0010: 16kHz - ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz - ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz - ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz - ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) + // 0001: 32kHz + ACCEL_ODR_32KHZ_SET = Bit0, + ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + ACCEL_ODR_16KHZ_SET = Bit1, + ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + ACCEL_ODR_8KHZ_SET = Bit1 | Bit0, + ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + ACCEL_ODR_1KHZ_SET = Bit2 | Bit1, + ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0, }; // GYRO_CONFIG1 diff --git a/src/drivers/imu/invensense/icm42688p/Kconfig b/src/drivers/imu/invensense/icm42688p/Kconfig new file mode 100644 index 000000000000..2c565ed51ee0 --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_ICM42688P + bool "icm42688p" + default n + ---help--- + Enable support for icm42688p \ No newline at end of file diff --git a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp index 42d90df8bf8e..618d57fea242 100644 --- a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp +++ b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void ICM42688P::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int icm42688p_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int icm42688p_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp index f287b2d13e23..768f4a3de925 100644 --- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp +++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp @@ -94,6 +94,10 @@ enum CONFIG_BIT : uint8_t { // GYRO_CONFIG enum GYRO_CONFIG_BIT : uint8_t { + XG_ST = Bit7, + YG_ST = Bit6, + ZG_ST = Bit5, + // FS_SEL [4:3] FS_SEL_250_DPS = 0, // 0b00000 FS_SEL_500_DPS = Bit3, // 0b01000 @@ -103,6 +107,10 @@ enum GYRO_CONFIG_BIT : uint8_t { // ACCEL_CONFIG enum ACCEL_CONFIG_BIT : uint8_t { + XA_ST = Bit7, + YA_ST = Bit6, + ZA_ST = Bit5, + // AFS_SEL [4:3] AFS_SEL_2G = 0, // 0b00000 AFS_SEL_4G = Bit3, // 0b01000 @@ -139,6 +147,7 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { // USER_CTRL enum USER_CTRL_BIT : uint8_t { FIFO_EN = Bit6, + I2C_MST_EN = Bit5, I2C_IF_DIS = Bit4, FIFO_RESET = Bit2, SIG_COND_RESET = Bit0, diff --git a/src/drivers/imu/invensense/mpu6000/Kconfig b/src/drivers/imu/invensense/mpu6000/Kconfig new file mode 100644 index 000000000000..ab64a6f32be7 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6000/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_MPU6000 + bool "mpu6000" + default n + ---help--- + Enable support for mpu6000 \ No newline at end of file diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 86c69ed07b57..701a330db9af 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -MPU6000::MPU6000(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +MPU6000::MPU6000(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -198,9 +197,16 @@ void MPU6000::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -221,7 +227,13 @@ void MPU6000::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = fifo_count / sizeof(FIFO::DATA); + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish @@ -229,7 +241,7 @@ void MPU6000::RunImpl() perf_count(_fifo_overflow_perf); } else if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -244,6 +256,7 @@ void MPU6000::RunImpl() // full reset if things are failing consistently if (_failure_count > 10) { + PX4_DEBUG("Full reset because things are failing consistently"); Reset(); return; } @@ -258,6 +271,7 @@ void MPU6000::RunImpl() } else { // register check failed, force reset perf_count(_bad_register_perf); + PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg); Reset(); } @@ -371,13 +385,10 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU6000::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -425,6 +436,7 @@ uint8_t MPU6000::RegisterRead(Register reg) uint8_t cmd[2] {}; cmd[0] = static_cast(reg) | DIR_READ; set_frequency(SPI_SPEED); // low speed for regular registers + px4_udelay(10); transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; } @@ -433,6 +445,7 @@ void MPU6000::RegisterWrite(Register reg, uint8_t value) { uint8_t cmd[2] { (uint8_t)reg, value }; set_frequency(SPI_SPEED); // low speed for regular registers + px4_udelay(10); transfer(cmd, cmd, sizeof(cmd)); } @@ -489,8 +502,8 @@ void MPU6000::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 86eb5b9ecc71..2d5180013a22 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_MPU6000; class MPU6000 : public device::SPI, public I2CSPIDriver { public: - MPU6000(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + MPU6000(const I2CSPIDriverConfig &config); ~MPU6000() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class MPU6000 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{8}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{8}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -143,8 +140,8 @@ class MPU6000 : public device::SPI, public I2CSPIDriver FIFO::DATA _fifo_sample_last_new_accel{}; uint32_t _fifo_accel_samples_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -152,12 +149,10 @@ class MPU6000 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{7}; diff --git a/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp b/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp index da095ee0f2f4..f1ed1b24be06 100644 --- a/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp +++ b/src/drivers/imu/invensense/mpu6000/mpu6000_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void MPU6000::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MPU6000 *instance = new MPU6000(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int mpu6000_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int mpu6000_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt index c11ca2d64cef..c8c14ac3d4ac 100644 --- a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-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 diff --git a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp index d1e04aa3b366..fee6661eb91f 100644 --- a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp +++ b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -64,6 +64,7 @@ static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C static constexpr float TEMPERATURE_OFFSET = 21.f; // C enum class Register : uint8_t { + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -86,6 +87,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG @@ -142,9 +152,9 @@ enum INT_ENABLE_BIT : uint8_t { // SIGNAL_PATH_RESET enum SIGNAL_PATH_RESET_BIT : uint8_t { - GYRO_RESET = Bit2, - ACCEL_RESET = Bit1, - TEMP_RESET = Bit0, + GYRO_RST = Bit2, + ACCEL_RST = Bit1, + TEMP_RST = Bit0, }; // USER_CTRL diff --git a/src/drivers/imu/invensense/mpu6500/Kconfig b/src/drivers/imu/invensense/mpu6500/Kconfig new file mode 100644 index 000000000000..b99ad79b9817 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_MPU6500 + bool "mpu6500" + default n + ---help--- + Enable support for mpu6500 \ No newline at end of file diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 9d5e7661771a..2f29db0f79c4 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -MPU6500::MPU6500(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_MPU6500, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +MPU6500::MPU6500(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (config.drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -106,6 +105,30 @@ void MPU6500::print_status() perf_print_counter(_drdy_missed_perf); } +bool MPU6500::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int MPU6500::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,9 +162,18 @@ void MPU6500::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Wakeup and reset digital signal path RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); - RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); + RegisterWrite(Register::SIGNAL_PATH_RESET, + SIGNAL_PATH_RESET_BIT::GYRO_RST | SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS); // if reset succeeded then configure @@ -197,9 +229,16 @@ void MPU6500::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -220,16 +259,21 @@ void MPU6500::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -371,13 +415,10 @@ int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU6500::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -489,8 +530,8 @@ void MPU6500::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 671d19165a96..7686db5114e5 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_MPU6500; class MPU6500 : public device::SPI, public I2CSPIDriver { public: - MPU6500(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + MPU6500(const I2CSPIDriverConfig &config); ~MPU6500() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class MPU6500 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -110,6 +107,7 @@ class MPU6500 : public device::SPI, public I2CSPIDriver bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -140,8 +138,8 @@ class MPU6500 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -149,15 +147,13 @@ class MPU6500 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{9}; + static constexpr uint8_t size_register_cfg{15}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, @@ -169,5 +165,11 @@ class MPU6500 : public device::SPI, public I2CSPIDriver { Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, USER_CTRL_BIT::I2C_IF_DIS }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp index 82f39b005cdd..99d25eb42408 100644 --- a/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp +++ b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,25 +46,6 @@ void MPU6500::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPU6500::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MPU6500 *instance = new MPU6500(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int mpu6500_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int mpu6500_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp index 904369fba4a0..57fc057239ed 100644 --- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp +++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -67,6 +67,7 @@ static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C static constexpr float TEMPERATURE_OFFSET = 21.f; // C enum class Register : uint8_t { + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -102,6 +103,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG @@ -122,7 +132,7 @@ enum GYRO_CONFIG_BIT : uint8_t { GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 // FCHOICE_B [1:0] - FCHOICE_B_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz + FCHOICE_B_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz }; // ACCEL_CONFIG diff --git a/src/drivers/imu/invensense/mpu9250/Kconfig b/src/drivers/imu/invensense/mpu9250/Kconfig new file mode 100644 index 000000000000..2bd2c8e1effb --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_MPU9250 + bool "mpu9250" + default n + ---help--- + Enable support for mpu9250 \ No newline at end of file diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 1433b8322151..501f5f97db9c 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -42,22 +42,23 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer) : - SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +MPU9250::MPU9250(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + bool enable_magnetometer = config.custom1 == 1; + if (enable_magnetometer) { - _slave_ak8963_magnetometer = new AKM_AK8963::MPU9250_AK8963(*this, rotation); + _slave_ak8963_magnetometer = new AKM_AK8963::MPU9250_AK8963(*this, config.rotation); if (_slave_ak8963_magnetometer) { for (auto &r : _register_cfg) { @@ -133,6 +134,30 @@ void MPU9250::print_status() } } +bool MPU9250::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int MPU9250::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -166,6 +191,14 @@ void MPU9250::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Wakeup and reset digital signal path RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); RegisterWrite(Register::SIGNAL_PATH_RESET, @@ -231,9 +264,16 @@ void MPU9250::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -254,16 +294,21 @@ void MPU9250::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -405,13 +450,10 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU9250::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -496,6 +538,11 @@ uint16_t MPU9250::FIFOReadCount() return combine(fifo_count_buf[1], fifo_count_buf[2]); } +static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) +{ + return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); +} + bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) { FIFOTransferBuffer buffer{}; @@ -507,9 +554,42 @@ bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } + uint8_t first_sample = 0; + + if (samples >= 4) { + if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) { + // [A0, A1, A2, A3] + // A0==A1, A2==A3 + first_sample = 1; - ProcessGyro(timestamp_sample, buffer.f, samples); - return ProcessAccel(timestamp_sample, buffer.f, samples); + } else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) { + // [A0, A1, A2, A3] + // A0, A1==A2, A3 + first_sample = 0; + + } else if (_slave_ak8963_magnetometer && fifo_accel_equal(buffer.f[2], buffer.f[3])) { + // if the slave I2C magnetometer is active we tolerate these missing samples, but only if intermittant + // [A0, A1, A2, A3] + // A0, A1, A2 == A3 + first_sample = 2; + samples -= 2; // skip first 2 samples + + } else { + // no matching accel samples is an error + if (!_slave_ak8963_magnetometer) { + // if the slave I2C magnetometer is active we tolerate these missing samples, but only if intermittant + // consecutive errors will still trigger a full sensor reset + perf_count(_bad_transfer_perf); + } + + return false; + } + } + + ProcessGyro(timestamp_sample, &buffer.f[first_sample], samples); + ProcessAccel(timestamp_sample, &buffer.f[first_sample], samples); + + return true; } void MPU9250::FIFOReset() @@ -523,8 +603,8 @@ void MPU9250::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO @@ -535,42 +615,14 @@ void MPU9250::FIFOReset() } } -static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) -{ - return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); -} - -bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +void MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) { sensor_accel_fifo_s accel{}; accel.timestamp_sample = timestamp_sample; accel.samples = 0; accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; - bool bad_data = false; - - // accel data is doubled in FIFO, but might be shifted - int accel_first_sample = 1; - - if (samples >= 4) { - if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) { - // [A0, A1, A2, A3] - // A0==A1, A2==A3 - accel_first_sample = 1; - - } else if (fifo_accel_equal(fifo[1], fifo[2])) { - // [A0, A1, A2, A3] - // A0, A1==A2, A3 - accel_first_sample = 0; - - } else { - // no matching accel samples is an error - bad_data = true; - perf_count(_bad_transfer_perf); - } - } - - for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) { + for (int i = 0; i < samples; i = i + SAMPLES_PER_TRANSFER) { int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L); int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L); int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L); @@ -589,8 +641,6 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA if (accel.samples > 0) { _px4_accel.updateFIFO(accel); } - - return !bad_data; } void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 05aaa3cd2682..369d2ec6e7cb 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -58,12 +58,9 @@ using namespace InvenSense_MPU9250; class MPU9250 : public device::SPI, public I2CSPIDriver { public: - MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer = false); + MPU9250(const I2CSPIDriverConfig &config); ~MPU9250() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -76,12 +73,12 @@ class MPU9250 : public device::SPI, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -112,6 +109,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -123,7 +121,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); void FIFOReset(); - bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); void UpdateTemperature(); @@ -152,8 +150,8 @@ class MPU9250 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -161,15 +159,13 @@ class MPU9250 : public device::SPI, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{12}; + static constexpr uint8_t size_register_cfg{18}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, @@ -184,5 +180,11 @@ class MPU9250 : public device::SPI, public I2CSPIDriver { Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp index 6428f77e09a8..e83124e500d3 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -51,7 +51,6 @@ MPU9250_AK8963::MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation) : _px4_mag(mpu9250.get_device_id(), rotation) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963); - _px4_mag.set_external(mpu9250.external()); // in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ _px4_mag.set_scale(1.5e-3f); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp index d6a7902450c6..73a87a71dbc3 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,15 +40,14 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -MPU9250_I2C::MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, int address, spi_drdy_gpio_t drdy_gpio) : - I2C(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +MPU9250_I2C::MPU9250_I2C(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { - if (drdy_gpio != 0) { + if (_drdy_gpio != 0) { _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); } @@ -198,9 +197,16 @@ void MPU9250_I2C::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { perf_count(_drdy_missed_perf); } @@ -221,16 +227,21 @@ void MPU9250_I2C::RunImpl() } else { // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure - const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * - SAMPLES_PER_TRANSFER; // round down to nearest + uint8_t samples = fifo_count / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); - } else if (samples >= 1) { - if (FIFORead(now, samples)) { + } else if (samples >= SAMPLES_PER_TRANSFER) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -372,13 +383,10 @@ int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU9250_I2C::DataReady() { - uint32_t expected = 0; - // at least the required number of samples in the FIFO - if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) - && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { - - _drdy_count.store(0); + if (++_drdy_count >= _fifo_gyro_samples) { + _drdy_timestamp_sample.store(hrt_absolute_time()); + _drdy_count -= _fifo_gyro_samples; ScheduleNow(); } } @@ -496,8 +504,8 @@ void MPU9250_I2C::FIFOReset() RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled - _drdy_count.store(0); - _drdy_fifo_read_samples.store(0); + _drdy_count = 0; + _drdy_timestamp_sample.store(0); // FIFO_EN: enable both gyro and accel // USER_CTRL: re-enable FIFO diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp index e45697a3e0f2..7af10d0b6076 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include @@ -56,12 +56,9 @@ using namespace InvenSense_MPU9250; class MPU9250_I2C : public device::I2C, public I2CSPIDriver { public: - MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - int address, spi_drdy_gpio_t drdy_gpio); + MPU9250_I2C(const I2CSPIDriverConfig &config); ~MPU9250_I2C() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -74,12 +71,12 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver // Sensor Configuration static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f}; - static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer + static constexpr int32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 1000 Hz gyro static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; // Transfer data struct FIFOTransferBuffer { @@ -139,8 +136,8 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_timestamp_sample{0}; + int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -148,12 +145,10 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{9}; diff --git a/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp index 4712376e0b74..8f58afcd96e1 100644 --- a/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -38,7 +38,7 @@ void MPU9250_I2C::print_usage() { - PRINT_MODULE_USAGE_NAME("mpu9520", "driver"); + PRINT_MODULE_USAGE_NAME("mpu9520_i2c", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -47,25 +47,6 @@ void MPU9250_I2C::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPU9250_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - MPU9250_I2C *instance = new MPU9250_I2C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.i2c_address, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int mpu9250_i2c_main(int argc, char *argv[]) { int ch; @@ -74,15 +55,15 @@ extern "C" int mpu9250_i2c_main(int argc, char *argv[]) cli.default_i2c_frequency = I2C_SPEED; cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp index 8c9636dad674..a5297fa9d85e 100644 --- a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -47,26 +47,6 @@ void MPU9250::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPU9250::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - bool mag = (cli.custom1 == 1); - MPU9250 *instance = new MPU9250(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO(), mag); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int mpu9250_main(int argc, char *argv[]) { int ch; @@ -74,19 +54,19 @@ extern "C" int mpu9250_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "MR:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "MR:")) != EOF) { switch (ch) { case 'M': cli.custom1 = 1; break; case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/l3gd20/Kconfig b/src/drivers/imu/l3gd20/Kconfig new file mode 100644 index 000000000000..417becfc95e6 --- /dev/null +++ b/src/drivers/imu/l3gd20/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_L3GD20 + bool "l3gd20" + default n + ---help--- + Enable support for l3gd20 \ No newline at end of file diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index 7e3ae8807826..152673048cb0 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -35,11 +35,10 @@ constexpr uint8_t L3GD20::_checked_registers[]; -L3GD20::L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_GYR_DEVTYPE_L3GD20, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_gyro(get_device_id(), rotation), +L3GD20::L3GD20(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_gyro(get_device_id(), config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), diff --git a/src/drivers/imu/l3gd20/L3GD20.hpp b/src/drivers/imu/l3gd20/L3GD20.hpp index 88ee5d677205..71bd532681e8 100644 --- a/src/drivers/imu/l3gd20/L3GD20.hpp +++ b/src/drivers/imu/l3gd20/L3GD20.hpp @@ -161,12 +161,9 @@ class L3GD20 : public device::SPI, public I2CSPIDriver { public: - L3GD20(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + L3GD20(const I2CSPIDriverConfig &config); ~L3GD20() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/imu/l3gd20/l3gd20_main.cpp b/src/drivers/imu/l3gd20/l3gd20_main.cpp index 91e805724eee..4c0a25ad2079 100644 --- a/src/drivers/imu/l3gd20/l3gd20_main.cpp +++ b/src/drivers/imu/l3gd20/l3gd20_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2019, 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 @@ -49,25 +49,6 @@ L3GD20::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *L3GD20::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - L3GD20 *instance = new L3GD20(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - void L3GD20::custom_method(const BusCLIArguments &cli) { switch (cli.custom1) { @@ -84,15 +65,15 @@ extern "C" int l3gd20_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/lsm303d/Kconfig b/src/drivers/imu/lsm303d/Kconfig new file mode 100644 index 000000000000..49c139db4f79 --- /dev/null +++ b/src/drivers/imu/lsm303d/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_LSM303D + bool "lsm303d" + default n + ---help--- + Enable support for lsm303d \ No newline at end of file diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index e4887f283510..7a43f9f0a768 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -53,19 +53,17 @@ static constexpr uint8_t _checked_registers[] = { ADDR_CTRL_REG7 }; -LSM303D::LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_LSM303D, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_mag(get_device_id(), rotation), +LSM303D::LSM303D(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_mag(get_device_id(), config.rotation), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d: bad_reg")), _bad_values(perf_alloc(PC_COUNT, "lsm303d: bad_val")), _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d: acc_dupe")) { - _px4_mag.set_external(external()); } LSM303D::~LSM303D() @@ -559,7 +557,6 @@ LSM303D::measureMagnetometer() _px4_mag.set_temperature(_last_temperature); _px4_mag.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values)); - _px4_mag.set_external(external()); _px4_mag.update(timestamp_sample, raw_mag_report.x, raw_mag_report.y, raw_mag_report.z); _mag_last_measure = timestamp_sample; diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp index 3687d4fc6eae..62d3de51224f 100644 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include #include #include #include @@ -141,12 +141,9 @@ class LSM303D : public device::SPI, public I2CSPIDriver { public: - LSM303D(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + LSM303D(const I2CSPIDriverConfig &config); ~LSM303D() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/imu/lsm303d/lsm303d_main.cpp b/src/drivers/imu/lsm303d/lsm303d_main.cpp index b80868c574dd..47220012e445 100644 --- a/src/drivers/imu/lsm303d/lsm303d_main.cpp +++ b/src/drivers/imu/lsm303d/lsm303d_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019, 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 @@ -52,25 +52,6 @@ LSM303D::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LSM303D::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LSM303D *instance = new LSM303D(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int lsm303d_main(int argc, char *argv[]) { int ch; @@ -78,15 +59,15 @@ extern "C" int lsm303d_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/imu/st/CMakeLists.txt b/src/drivers/imu/st/CMakeLists.txt deleted file mode 100644 index 73a9349572d9..000000000000 --- a/src/drivers/imu/st/CMakeLists.txt +++ /dev/null @@ -1,34 +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. -# -############################################################################ - -add_subdirectory(lsm9ds1) diff --git a/src/drivers/imu/st/Kconfig b/src/drivers/imu/st/Kconfig new file mode 100644 index 000000000000..9047657caa94 --- /dev/null +++ b/src/drivers/imu/st/Kconfig @@ -0,0 +1,3 @@ +menu "ST" +rsource "*/Kconfig" +endmenu #Invensense diff --git a/src/drivers/imu/st/lsm9ds1/Kconfig b/src/drivers/imu/st/lsm9ds1/Kconfig new file mode 100644 index 000000000000..34bc50c2f79c --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ST_LSM9DS1 + bool "lsm9ds1" + default n + ---help--- + Enable support for lsm9ds1 diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index fc98fc2793c7..b5577eff632a 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) +LSM9DS1::LSM9DS1(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) { ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); } @@ -171,11 +170,13 @@ void LSM9DS1::RunImpl() break; case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + // always check current FIFO count bool success = false; // Number of unread words (16-bit axes) stored in FIFO. const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC); - const uint8_t samples = FIFO_SRC & static_cast(FIFO_SRC_BIT::FSS); + uint8_t samples = FIFO_SRC & static_cast(FIFO_SRC_BIT::FSS); if (FIFO_SRC & FIFO_SRC_BIT::OVRN) { // overflow @@ -186,13 +187,19 @@ void LSM9DS1::RunImpl() perf_count(_fifo_empty_perf); } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + if (samples > FIFO_MAX_SAMPLES) { // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); } else if (samples >= 1) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp index 15bbc3be6e9e..c02688f0e715 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include @@ -55,12 +55,9 @@ using namespace ST_LSM9DS1; class LSM9DS1 : public device::SPI, public I2CSPIDriver { public: - LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + LSM9DS1(const I2CSPIDriverConfig &config); ~LSM9DS1() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -77,7 +74,7 @@ class LSM9DS1 : public device::SPI, public I2CSPIDriver static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo - static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; struct register_config_t { Register reg; @@ -126,7 +123,7 @@ class LSM9DS1 : public device::SPI, public I2CSPIDriver } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{6}; diff --git a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp index 24dbc8d33152..bdbb1c9355ee 100644 --- a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp +++ b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -46,25 +46,6 @@ void LSM9DS1::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LSM9DS1::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LSM9DS1 *instance = new LSM9DS1(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int lsm9ds1_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int lsm9ds1_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/irlock/Kconfig b/src/drivers/irlock/Kconfig new file mode 100644 index 000000000000..ddf22968cdaa --- /dev/null +++ b/src/drivers/irlock/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IRLOCK + bool "irlock" + default n + ---help--- + Enable support for irlock \ No newline at end of file diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 25e3bbf6b6c1..c8428ae2ef11 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -92,11 +92,9 @@ struct irlock_s { class IRLOCK : public device::I2C, public I2CSPIDriver { public: - IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address); + IRLOCK(const I2CSPIDriverConfig &config); ~IRLOCK() override = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; @@ -119,9 +117,9 @@ class IRLOCK : public device::I2C, public I2CSPIDriver uORB::Publication _irlock_report_topic{ORB_ID(irlock_report)}; }; -IRLOCK::IRLOCK(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) : - I2C(DRV_SENS_DEVTYPE_IRLOCK, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) +IRLOCK::IRLOCK(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) { } @@ -272,24 +270,6 @@ void IRLOCK::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *IRLOCK::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - IRLOCK *instance = new IRLOCK(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" __EXPORT int irlock_main(int argc, char *argv[]) { using ThisDriver = IRLOCK; diff --git a/src/drivers/irlock/parameters.c b/src/drivers/irlock/parameters.c new file mode 100644 index 000000000000..313516579356 --- /dev/null +++ b/src/drivers/irlock/parameters.c @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * IR-LOCK Sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_IRLOCK, 0); diff --git a/src/drivers/lights/Kconfig b/src/drivers/lights/Kconfig new file mode 100644 index 000000000000..821cd0130879 --- /dev/null +++ b/src/drivers/lights/Kconfig @@ -0,0 +1,10 @@ +menu "Lights" + menuconfig COMMON_LIGHT + bool "Common Light's" + default n + select DRIVERS_LIGHTS_RGBLED + select DRIVERS_LIGHTS_RGBLED_NCP5623C + ---help--- + Enable default set of light drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/lights/neopixel/Kconfig b/src/drivers/lights/neopixel/Kconfig new file mode 100644 index 000000000000..a8f68316904b --- /dev/null +++ b/src/drivers/lights/neopixel/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LIGHTS_NEOPIXEL + bool "neopixel" + default n + ---help--- + Enable support for neopixel \ No newline at end of file diff --git a/src/drivers/lights/rgbled/Kconfig b/src/drivers/lights/rgbled/Kconfig new file mode 100644 index 000000000000..52f606acd7fb --- /dev/null +++ b/src/drivers/lights/rgbled/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LIGHTS_RGBLED + bool "rgbled" + default n + ---help--- + Enable support for rgbled \ No newline at end of file diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index d7f0063b34c5..1780978af47d 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/src/drivers/lights/rgbled/rgbled.cpp @@ -48,9 +48,6 @@ #include #include #include -#include -#include -#include using namespace time_literals; @@ -68,11 +65,9 @@ using namespace time_literals; class RGBLED : public device::I2C, public I2CSPIDriver { public: - RGBLED(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address); + RGBLED(const I2CSPIDriverConfig &config); virtual ~RGBLED() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; @@ -80,31 +75,25 @@ class RGBLED : public device::I2C, public I2CSPIDriver void RunImpl(); -protected: - void print_status() override; private: + void print_status() override; + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); float _brightness{1.0f}; - float _max_brightness{1.0f}; uint8_t _r{0}; uint8_t _g{0}; uint8_t _b{0}; bool _leds_enabled{true}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - LedController _led_controller; - - int send_led_enable(bool enable); - int send_led_rgb(); - int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); - void update_params(); }; -RGBLED::RGBLED(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) : - I2C(DRV_LED_DEVTYPE_RGBLED, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) +RGBLED::RGBLED(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) { } @@ -121,8 +110,6 @@ RGBLED::init() send_led_enable(false); send_led_rgb(); - update_params(); - // kick off work queue ScheduleNow(); @@ -136,25 +123,13 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - /** - this may look strange, but is needed. There is a serial - EEPROM (Microchip-24aa01) that responds to a bunch of I2C - addresses, including the 0x55 used by this LED device. So - we need to do enough operations to be sure we are talking - to the right device. These 3 operations seem to be enough, - as the 3rd one consistently fails if no RGBLED is on the bus. - */ - - unsigned prevretries = _retries; - _retries = 4; - if ((ret = get(on, powersave, r, g, b)) != OK || (ret = send_led_enable(false) != OK) || (ret = send_led_enable(false) != OK)) { return ret; } - _retries = prevretries; + _retries = 1; return ret; } @@ -180,19 +155,6 @@ RGBLED::print_status() void RGBLED::RunImpl() { - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - update_params(); - - // Immediately update to change brightness - send_led_rgb(); - } - LedControlData led_control_data; if (_led_controller.update(led_control_data) == 1) { @@ -281,9 +243,9 @@ RGBLED::send_led_rgb() { /* To scale from 0..255 -> 0..15 shift right by 4 bits */ const uint8_t msg[6] = { - SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness * _max_brightness + 0.5f), - SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness * _max_brightness + 0.5f), - SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness * _max_brightness + 0.5f) + SUB_ADDR_PWM0, static_cast((_b >> 4) * _brightness + 0.5f), + SUB_ADDR_PWM1, static_cast((_g >> 4) * _brightness + 0.5f), + SUB_ADDR_PWM2, static_cast((_r >> 4) * _brightness + 0.5f) }; return transfer(msg, sizeof(msg), nullptr, 0); } @@ -308,22 +270,6 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) return ret; } -void -RGBLED::update_params() -{ - int32_t maxbrt = 15; - param_get(param_find("LED_RGB_MAXBRT"), &maxbrt); - maxbrt = maxbrt > 15 ? 15 : maxbrt; - maxbrt = maxbrt < 0 ? 0 : maxbrt; - - // A minimum of 2 "on" steps is required for breathe effect - if (maxbrt == 1) { - maxbrt = 2; - } - - _max_brightness = maxbrt / 15.0f; -} - void RGBLED::print_usage() { @@ -334,24 +280,6 @@ RGBLED::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *RGBLED::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - RGBLED *instance = new RGBLED(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" __EXPORT int rgbled_main(int argc, char *argv[]) { using ThisDriver = RGBLED; diff --git a/src/drivers/lights/rgbled/rgbled_params.c b/src/drivers/lights/rgbled/rgbled_params.c deleted file mode 100644 index 641e77202d57..000000000000 --- a/src/drivers/lights/rgbled/rgbled_params.c +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 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 rgbled_params.c - * - * Parameters defined by the RBG led driver - * - * @author Nate Weibley - */ - - -#include -#include - -/** - * RGB Led brightness limit - * - * Set to 0 to disable, 1 for minimum brightness up to 15 (max) - * - * @min 0 - * @max 15 - * @group System - */ -PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15); diff --git a/src/drivers/lights/rgbled_ncp5623c/Kconfig b/src/drivers/lights/rgbled_ncp5623c/Kconfig new file mode 100644 index 000000000000..14964779b141 --- /dev/null +++ b/src/drivers/lights/rgbled_ncp5623c/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LIGHTS_RGBLED_NCP5623C + bool "rgbled_ncp5623c" + default n + ---help--- + Enable support for rgbled_ncp5623c \ No newline at end of file diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index 6ee9ed53d37a..7b1f62b563c4 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -34,7 +34,8 @@ /** * @file rgbled_ncp5623c.cpp * - * Driver for the onboard RGB LED controller (NCP5623C) connected via I2C. + * Driver for the onboard RGB LED controller (NCP5623B or NCP5623C) + * connected via I2C. * * @author CUAVcaijie */ @@ -43,71 +44,91 @@ #include #include -#include #include #include #include -#include -#include using namespace time_literals; -#define ADDR 0x39 /**< I2C adress of NCP5623C */ +#define NCP5623B_ADDR 0x38 /**< I2C address of NCP5623B */ +#define NCP5623C_ADDR 0x39 /**< I2C address of NCP5623C */ -#define NCP5623_LED_CURRENT 0x20 /**< Current register */ -#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ -#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ -#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ +#define NCP5623_LED_CURRENT 0x20 /**< Current register */ +#define NCP5623_LED_PWM0 0x40 /**< pwm0 register */ +#define NCP5623_LED_PWM1 0x60 /**< pwm1 register */ +#define NCP5623_LED_PWM2 0x80 /**< pwm2 register */ -#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ -#define NCP5623_LED_OFF 0x00 /**< off */ +#define NCP5623_LED_BRIGHT 0x1f /**< full brightness */ +#define NCP5623_LED_OFF 0x00 /**< off */ -class RGBLED_NPC5623C : public device::I2C, public I2CSPIDriver +class RGBLED_NCP5623C : public device::I2C, public I2CSPIDriver { public: - RGBLED_NPC5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address); - virtual ~RGBLED_NPC5623C() = default; + RGBLED_NCP5623C(const I2CSPIDriverConfig &config); + virtual ~RGBLED_NCP5623C() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; int probe() override; void RunImpl(); + virtual int8_t get_i2c_address() {return get_device_address();} private: + int send_led_rgb(); + + int write(uint8_t reg, uint8_t data); float _brightness{1.0f}; - float _max_brightness{1.0f}; - uint8_t _r{0}; - uint8_t _g{0}; - uint8_t _b{0}; + uint8_t _r{0}; + uint8_t _g{0}; + uint8_t _b{0}; volatile bool _running{false}; volatile bool _should_run{true}; bool _leds_enabled{true}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - LedController _led_controller; - int send_led_rgb(); - void update_params(); - - int write(uint8_t reg, uint8_t data); + uint8_t _red{NCP5623_LED_PWM0}; + uint8_t _green{NCP5623_LED_PWM1}; + uint8_t _blue{NCP5623_LED_PWM2}; }; -RGBLED_NPC5623C::RGBLED_NPC5623C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, const int address) : - I2C(DRV_LED_DEVTYPE_RGBLED_NCP5623C, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) +RGBLED_NCP5623C::RGBLED_NCP5623C(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) { + int ordering = config.custom1; + // ordering is RGB: Hundreds is Red, Tens is green and ones is Blue + // 123 would drive the + // R LED from = NCP5623_LED_PWM0 + // G LED from = NCP5623_LED_PWM1 + // B LED from = NCP5623_LED_PWM2 + // 321 would drive the + // R LED from = NCP5623_LED_PWM2 + // G LED from = NCP5623_LED_PWM1 + // B LED from = NCP5623_LED_PWM0 + const uint8_t sig[] = {NCP5623_LED_PWM0, NCP5623_LED_PWM1, NCP5623_LED_PWM2}; + // Process ordering in lsd to msd order.(BGR) + uint8_t *color[] = {&_blue, &_green, &_red }; + unsigned int s = 0; + + for (unsigned int i = 0; i < arraySize(color); i++) { + s = (ordering % 10) - 1; + + if (s < arraySize(sig)) { + *color[i] = sig[s]; + } + + ordering /= 10; + } } int -RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) +RGBLED_NCP5623C::write(uint8_t reg, uint8_t data) { uint8_t msg[1] = { 0x00 }; msg[0] = ((reg & 0xe0) | (data & 0x1f)); @@ -118,7 +139,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) } int -RGBLED_NPC5623C::init() +RGBLED_NCP5623C::init() { int ret = I2C::init(); @@ -126,8 +147,6 @@ RGBLED_NPC5623C::init() return ret; } - update_params(); - _running = true; ScheduleNow(); @@ -136,29 +155,22 @@ RGBLED_NPC5623C::init() } int -RGBLED_NPC5623C::probe() +RGBLED_NCP5623C::probe() { - _retries = 4; + _retries = 2; + int status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF); + + if (status == PX4_ERROR) { + set_device_address(NCP5623B_ADDR); + status = write(NCP5623_LED_CURRENT, NCP5623_LED_OFF); + } - return write(NCP5623_LED_CURRENT, 0x00); + return status; } void -RGBLED_NPC5623C::RunImpl() +RGBLED_NCP5623C::RunImpl() { - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - update_params(); - - // Immediately update to change brightness - send_led_rgb(); - } - LedControlData led_control_data; if (_led_controller.update(led_control_data) == 1) { @@ -210,72 +222,48 @@ RGBLED_NPC5623C::RunImpl() * Send RGB PWM settings to LED driver according to current color and brightness */ int -RGBLED_NPC5623C::send_led_rgb() +RGBLED_NCP5623C::send_led_rgb() { - uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80}; - uint8_t brightness = 0x1f * _max_brightness; + uint8_t brightness = UINT8_MAX; msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f); - msg[2] = NCP5623_LED_PWM0 | (uint8_t(_r * _brightness) & 0x1f); - msg[4] = NCP5623_LED_PWM1 | (uint8_t(_g * _brightness) & 0x1f); - msg[6] = NCP5623_LED_PWM2 | (uint8_t(_b * _brightness) & 0x1f); + msg[2] = _red | (uint8_t(_r * _brightness) & 0x1f); + msg[4] = _green | (uint8_t(_g * _brightness) & 0x1f); + msg[6] = _blue | (uint8_t(_b * _brightness) & 0x1f); return transfer(&msg[0], 7, nullptr, 0); } void -RGBLED_NPC5623C::update_params() -{ - int32_t maxbrt = 31; - param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt); - maxbrt = maxbrt > 31 ? 31 : maxbrt; - maxbrt = maxbrt < 0 ? 0 : maxbrt; - - if (maxbrt == 0) { - maxbrt = 1; - } - - _max_brightness = maxbrt / 31.0f; -} - -void -RGBLED_NPC5623C::print_usage() +RGBLED_NCP5623C::print_usage() { PRINT_MODULE_USAGE_NAME("rgbled", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39); + PRINT_MODULE_USAGE_PARAM_INT('o', 123, 123, 321, "RGB PWM Assignment", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *RGBLED_NPC5623C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - RGBLED_NPC5623C *instance = new RGBLED_NPC5623C(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, - cli.i2c_address); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]) { - using ThisDriver = RGBLED_NPC5623C; + using ThisDriver = RGBLED_NCP5623C; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 100000; - cli.i2c_address = ADDR; + cli.i2c_address = NCP5623C_ADDR; + cli.custom1 = 123; + int ch; + + while ((ch = cli.getOpt(argc, argv, "o:")) != EOF) { + switch (ch) { + case 'o': + cli.custom1 = atoi(cli.optArg()); + break; + } + } - const char *verb = cli.parseDefaultArguments(argc, argv); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c deleted file mode 100755 index a834cae4cb66..000000000000 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c_params.c +++ /dev/null @@ -1,56 +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 rgbled_ncp5623c_params.c - * - * Parameters defined by the RBG led driver - * - * @author CUAVcaijie - */ - - -#include -#include - -/** - * RGB Led brightness limit - * - * Set to 0 to disable, 1 for minimum brightness up to 31 (max) - * - * @min 0 - * @max 31 - * @group System - */ -PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31); - diff --git a/src/drivers/lights/rgbled_pwm/Kconfig b/src/drivers/lights/rgbled_pwm/Kconfig new file mode 100644 index 000000000000..231b5161d157 --- /dev/null +++ b/src/drivers/lights/rgbled_pwm/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LIGHTS_RGBLED_PWM + bool "rgbled_pwm" + default n + ---help--- + Enable support for rgbled_pwm \ No newline at end of file diff --git a/src/drivers/linux_pwm_out/CMakeLists.txt b/src/drivers/linux_pwm_out/CMakeLists.txt index 38ee56b94a6f..f6dbb3bbdea2 100644 --- a/src/drivers/linux_pwm_out/CMakeLists.txt +++ b/src/drivers/linux_pwm_out/CMakeLists.txt @@ -37,7 +37,8 @@ px4_add_module( COMPILE_FLAGS SRCS linux_pwm_out.cpp + MODULE_CONFIG + module.yaml DEPENDS - output_limit ) diff --git a/src/drivers/linux_pwm_out/Kconfig b/src/drivers/linux_pwm_out/Kconfig new file mode 100644 index 000000000000..36193d19745d --- /dev/null +++ b/src/drivers/linux_pwm_out/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_LINUX_PWM_OUT + bool "linux_pwm_out" + default n + ---help--- + Enable support for linux_pwm_out \ No newline at end of file diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index d19da24c6700..e57d7d63699c 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-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 @@ -36,6 +36,8 @@ #include #include +#include + using namespace pwm_out; LinuxPWMOut::LinuxPWMOut() : @@ -44,8 +46,10 @@ LinuxPWMOut::LinuxPWMOut() : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + if (!_mixing_output.useDynamicMixing()) { + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + } } LinuxPWMOut::~LinuxPWMOut() @@ -130,6 +134,8 @@ void LinuxPWMOut::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); perf_count(_interval_perf); @@ -154,8 +160,8 @@ void LinuxPWMOut::update_params() { updateParams(); - // skip update when armed - if (_mixing_output.armed().armed) { + // skip update when armed or dynamic mixing enabled + if (_mixing_output.armed().armed || _mixing_output.useDynamicMixing()) { return; } @@ -299,21 +305,21 @@ void LinuxPWMOut::update_params() int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) { + SmartLock lock_guard(_lock); + int ret = OK; PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg); - lock(); - switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); update_params(); break; } @@ -323,8 +329,6 @@ int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - unlock(); - if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); } diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.hpp b/src/drivers/linux_pwm_out/linux_pwm_out.hpp index 79755dcd6924..37060b728057 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.hpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.hpp @@ -85,7 +85,7 @@ class LinuxPWMOut : public cdev::CDev, public ModuleBase, public Ou void update_params(); - MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; + MixingOutput _mixing_output{"PWM_MAIN", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/drivers/linux_pwm_out/module.yaml b/src/drivers/linux_pwm_out/module.yaml new file mode 100644 index 000000000000..133e04e155bb --- /dev/null +++ b/src/drivers/linux_pwm_out/module.yaml @@ -0,0 +1,11 @@ +module_name: PWM Output +actuator_output: + output_groups: + - param_prefix: PWM_MAIN + channel_label: 'Channel' + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } + num_channels: 8 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 84bbbe89244e..5bb603529a86 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017, 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 @@ -40,3 +40,4 @@ add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) add_subdirectory(rm3100) +add_subdirectory(vtrantech) diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig new file mode 100644 index 000000000000..12859150806c --- /dev/null +++ b/src/drivers/magnetometer/Kconfig @@ -0,0 +1,20 @@ +menu "Magnetometer" + menuconfig COMMON_MAGNETOMETER + bool "Common Magnetometer's" + default n + select DRIVERS_MAGNETOMETER_AKM_AK8963 + select DRIVERS_MAGNETOMETER_AKM_AK09916 + select DRIVERS_MAGNETOMETER_BOSCH_BMM150 + select DRIVERS_MAGNETOMETER_HMC5883 + select DRIVERS_MAGNETOMETER_QMC5883L + select DRIVERS_MAGNETOMETER_ISENTEK_IST8308 + select DRIVERS_MAGNETOMETER_ISENTEK_IST8310 + select DRIVERS_MAGNETOMETER_LIS2MDL + select DRIVERS_MAGNETOMETER_LIS3MDL + select DRIVERS_MAGNETOMETER_LSM303AGR + select DRIVERS_MAGNETOMETER_RM3100 + select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L + ---help--- + Enable default set of magnetometer drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/akm/Kconfig b/src/drivers/magnetometer/akm/Kconfig new file mode 100644 index 000000000000..f40d0a6408bd --- /dev/null +++ b/src/drivers/magnetometer/akm/Kconfig @@ -0,0 +1,3 @@ +menu "AKM" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index da79675638df..613d9e184d36 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -AK09916::AK09916(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_AK09916, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +AK09916::AK09916(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } AK09916::~AK09916() diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 90046dcb8c10..98ad0dcb9aef 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -53,11 +53,9 @@ using namespace AKM_AK09916; class AK09916 : public device::I2C, public I2CSPIDriver { public: - AK09916(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + AK09916(const I2CSPIDriverConfig &config); ~AK09916() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/akm/ak09916/Kconfig b/src/drivers/magnetometer/akm/ak09916/Kconfig new file mode 100644 index 000000000000..6802acc38120 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak09916/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_AKM_AK09916 + bool "ak09916" + default n + ---help--- + Enable support for ak09916 diff --git a/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp index 47704d754b99..01d5cce823bd 100644 --- a/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp +++ b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -36,31 +36,13 @@ #include #include -I2CSPIDriverBase *AK09916::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - AK09916 *instance = new AK09916(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void AK09916::print_usage() { PRINT_MODULE_USAGE_NAME("ak09916", "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_ADDRESS(0x0c); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -71,16 +53,17 @@ extern "C" __EXPORT int ak09916_main(int argc, char *argv[]) using ThisDriver = AK09916; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp index b1722c25d914..8055f76149a4 100644 --- a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -AK8963::AK8963(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_AK8963, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +AK8963::AK8963(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } AK8963::~AK8963() diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.hpp b/src/drivers/magnetometer/akm/ak8963/AK8963.hpp index a0f1881c9c01..cf80d686218e 100644 --- a/src/drivers/magnetometer/akm/ak8963/AK8963.hpp +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.hpp @@ -53,11 +53,9 @@ using namespace AKM_AK8963; class AK8963 : public device::I2C, public I2CSPIDriver { public: - AK8963(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + AK8963(const I2CSPIDriverConfig &config); ~AK8963() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/akm/ak8963/Kconfig b/src/drivers/magnetometer/akm/ak8963/Kconfig new file mode 100644 index 000000000000..216621100fd0 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_AKM_AK8963 + bool "ak8963" + default n + ---help--- + Enable support for ak8963 diff --git a/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp b/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp index 6251986932f7..206275ccdc65 100644 --- a/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp +++ b/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -36,31 +36,13 @@ #include #include -I2CSPIDriverBase *AK8963::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - AK8963 *instance = new AK8963(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void AK8963::print_usage() { PRINT_MODULE_USAGE_NAME("ak8963", "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_ADDRESS(0x0c); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -71,16 +53,17 @@ extern "C" __EXPORT int ak8963_main(int argc, char *argv[]) using ThisDriver = AK8963; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/bosch/Kconfig b/src/drivers/magnetometer/bosch/Kconfig new file mode 100644 index 000000000000..a12dbffcfe39 --- /dev/null +++ b/src/drivers/magnetometer/bosch/Kconfig @@ -0,0 +1,3 @@ +menu "Bosch" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp index 214b57ac0fad..d761ae513f6a 100644 --- a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp +++ b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -35,20 +35,20 @@ using namespace time_literals; -BMM150::BMM150(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_BMM150, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +BMM150::BMM150(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } BMM150::~BMM150() { - perf_free(_reset_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); + perf_free(_reset_perf); perf_free(_overflow_perf); + perf_free(_self_test_failed_perf); } int BMM150::init() @@ -80,21 +80,25 @@ void BMM150::print_status() perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_overflow_perf); + perf_print_counter(_self_test_failed_perf); } int BMM150::probe() { - const uint8_t POWER_CONTROL = RegisterRead(Register::POWER_CONTROL); - const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + // 3 retries + for (int i = 0; i < 3; i++) { + const uint8_t POWER_CONTROL = RegisterRead(Register::POWER_CONTROL); + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); - PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID); + PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID); - // either power control bit is set and chip ID can be read, or both registers are 0x00 - if ((POWER_CONTROL & POWER_CONTROL_BIT::PowerControl) && (CHIP_ID == chip_identification_number)) { - return PX4_OK; + if (CHIP_ID == chip_identification_number) { + return PX4_OK; - } else if ((POWER_CONTROL == 0) && (CHIP_ID == 0)) { - return PX4_OK; + } else if ((CHIP_ID == 0) && !(POWER_CONTROL & POWER_CONTROL_BIT::PowerControl)) { + // in suspend Chip ID read (register 0x40) returns “0x00” (I²C) or high-Z (SPI). + return PX4_OK; + } } return PX4_ERROR; @@ -104,15 +108,18 @@ float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall) { float retval = 0; - // Processing compensation equations - // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25 - float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.f / data_rhall); - retval = (process_comp_x0 - 16384.f); - float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f); - float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.f; - float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.f; - float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.f) * process_comp_x3); - retval = ((process_comp_x4 / 8192.f) + (((float)_trim_data.dig_x1) * 8.f)) / 16.f; + // Overflow condition check + if ((mag_data_x != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) { + // Processing compensation equations + // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25 + float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall); + retval = (process_comp_x0 - 16384.0f); + float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f); + float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f; + float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.0f; + float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.0f) * process_comp_x3); + retval = ((process_comp_x4 / 8192.0f) + (((float)_trim_data.dig_x1) * 8.0f)) / 16.0f; + } return retval; } @@ -121,15 +128,17 @@ float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall) { float retval = 0; - // Processing compensation equations - // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1660-L1667 as of 2020-09-25 - float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.f / data_rhall; - retval = process_comp_y0 - 16384.f; - float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f); - float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.f; - float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f; - float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.f) * process_comp_y3); - retval = ((process_comp_y4 / 8192.f) + (((float)_trim_data.dig_y1) * 8.f)) / 16.f; + // Overflow condition check + if ((mag_data_y != OVERFLOW_XYAXES) && (data_rhall != 0) && (_trim_data.dig_xyz1 != 0)) { + // Processing compensation equations + float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.0f / data_rhall; + retval = process_comp_y0 - 16384.0f; + float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.0f); + float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.0f; + float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f; + float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.0f) * process_comp_y3); + retval = ((process_comp_y4 / 8192.0f) + (((float)_trim_data.dig_y1) * 8.0f)) / 16.0f; + } return retval; } @@ -138,35 +147,52 @@ float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall) { float retval = 0; - // Processing compensation equations - // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25 - float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4); - float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1); - float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1); - float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.f; - float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3; - float process_comp_z5 = (process_comp_z0 * 131072.f) - process_comp_z2; - retval = (process_comp_z5 / ((process_comp_z4) * 4.f)) / 16.f; + // Overflow condition check + if ((mag_data_z != OVERFLOW_ZAXIS) + && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0) && (_trim_data.dig_xyz1 != 0) + && (data_rhall != 0)) { + // Processing compensation equations + // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25 + float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4); + float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1); + float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1); + float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.0f; + float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3; + float process_comp_z5 = (process_comp_z0 * 131072.0f) - process_comp_z2; + retval = (process_comp_z5 / ((process_comp_z4) * 4.0f)) / 16.0f; + } return retval; } static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb) { - int16_t x = ((msb << 8) | lsb); - return x / 8; // arithmetic shift by 3 (13 bit signed integer) + // msb: 8-bit MSB part [12:5] of the 13 bit output data + // lsb: 5-bit LSB part [4:0] of the 13 bit output data + int16_t msb_data = ((int16_t)((int8_t)msb)) << 5; + int16_t lsb_data = ((lsb & 0xF8) >> 3); + + return (int16_t)(msb_data | lsb_data); } static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb) { - int16_t z = ((msb << 8) | lsb); - return z / 2; // arithmetic shift by 1 (15 bit signed integer) + // msb: 8-bit MSB part [12:5] of the 13 bit output data + // lsb: 7-bit LSB part [6:0] of the 15 bit output data + int16_t msb_data = ((int16_t)((int8_t)msb)) << 7; + int16_t lsb_data = ((lsb & 0xFE) >> 1); + + return (int16_t)(msb_data | lsb_data); } static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb) { - uint16_t rhall = ((msb << 8) | lsb); - return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer + // msb: 8-bit MSB part [13:6] of the 14 bit output data + // lsb: 6-bit LSB part [5:0] of the 14 bit output data + uint16_t msb_data = ((uint16_t)((uint16_t)msb)) << 6; + uint16_t lsb_data = ((lsb & 0xFC) >> 2); + + return (uint16_t)(msb_data | lsb_data); } void BMM150::RunImpl() @@ -181,14 +207,15 @@ void BMM150::RunImpl() _failure_count = 0; _state = STATE::WAIT_FOR_RESET; perf_count(_reset_perf); - ScheduleDelayed(3_ms); // 3.0 ms start-up time from suspend to sleep + ScheduleDelayed(10_ms); // 3.0 ms start-up time from suspend to sleep break; case STATE::WAIT_FOR_RESET: // Soft reset always brings the device into sleep mode (power off -> suspend -> sleep) if ((RegisterRead(Register::CHIP_ID) == chip_identification_number) - && (RegisterRead(Register::POWER_CONTROL) == POWER_CONTROL_BIT::PowerControl) + && (RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::PowerControl) + && !(RegisterRead(Register::POWER_CONTROL) & POWER_CONTROL_BIT::SoftReset) && (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) { // if reset succeeded then start self test @@ -216,23 +243,46 @@ void BMM150::RunImpl() // After performing self test OpMode "Self test" bit is set to 0 const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0); - // When self-test is successful, the corresponding self-test result bits are set - // “X-Self-Test” register 0x42 bit0 - // “Y-Self-Test” register 0x44 bit0 - // “Z-Self-Test” register 0x46 bit0 - const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0; - const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0; - const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0; - - if (opmode_self_test_cleared && (!x_success || !y_success || !z_success)) { - PX4_DEBUG("self test failed, resetting"); - perf_count(_self_test_failed_perf); - _state = STATE::RESET; - ScheduleDelayed(1000_ms); + if (opmode_self_test_cleared) { + // When self-test is successful, the corresponding self-test result bits are set + // “X-Self-Test” register 0x42 bit0 + // “Y-Self-Test” register 0x44 bit0 + // “Z-Self-Test” register 0x46 bit0 + const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0; + const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0; + const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0; + + if (x_success && y_success && z_success) { + _state = STATE::READ_TRIM; + ScheduleDelayed(10_ms); + + } else { + if (perf_event_count(_self_test_failed_perf) >= 5) { + PX4_ERR("self test still failing after 5 attempts"); + + // reluctantly proceed + _state = STATE::READ_TRIM; + ScheduleDelayed(10_ms); + + } else { + PX4_ERR("self test failed, resetting"); + perf_count(_self_test_failed_perf); + _state = STATE::RESET; + ScheduleDelayed(1_s); + } + } } else { - _state = STATE::READ_TRIM; - ScheduleDelayed(1_ms); + if (hrt_elapsed_time(&_reset_timestamp) < 3_s) { + // self test not complete, check again in 100 milliseconds + _state = STATE::SELF_TEST_CHECK; + ScheduleDelayed(100_ms); + + } else { + // full reset + _state = STATE::RESET; + ScheduleDelayed(10_ms); + } } } @@ -349,7 +399,8 @@ void BMM150::RunImpl() perf_count(_overflow_perf); } else { - _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); + _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + + perf_event_count(_bad_transfer_perf) + perf_event_count(_self_test_failed_perf)); _px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall)); success = true; diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp index 97ed6cf159b8..093a893d9d89 100644 --- a/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp +++ b/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -53,11 +53,9 @@ using namespace Bosch_BMM150; class BMM150 : public device::I2C, public I2CSPIDriver { public: - BMM150(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + BMM150(const I2CSPIDriverConfig &config); ~BMM150() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -130,7 +128,7 @@ class BMM150 : public device::I2C, public I2CSPIDriver register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset }, - { Register::OP_MODE, OP_MODE_BIT::ODR_20Hz, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test }, + { Register::OP_MODE, OP_MODE_BIT::ODR_20HZ_SET, OP_MODE_BIT::ODR_20HZ_CLEAR | OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test }, { Register::REPXY, REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR }, { Register::REPZ, REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR }, }; diff --git a/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp b/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp index a3e89fa837e3..78c128ff4c23 100644 --- a/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp +++ b/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -96,11 +96,12 @@ enum POWER_CONTROL_BIT : uint8_t { // OP_MODE enum OP_MODE_BIT : uint8_t { // 5:3 Data rate control - ODR_20Hz = Bit5 | Bit3, // ODR 20 Hz + ODR_20HZ_SET = Bit5 | Bit3, + ODR_20HZ_CLEAR = Bit4, // 2:1 Operation mode control - Opmode_Sleep = Bit2 | Bit1, // Sleep mode - Self_Test = Bit0, + Opmode_Sleep = Bit2 | Bit1, // Sleep mode + Self_Test = Bit0, }; // STATUS diff --git a/src/drivers/magnetometer/bosch/bmm150/Kconfig b/src/drivers/magnetometer/bosch/bmm150/Kconfig new file mode 100644 index 000000000000..e44e684bf869 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm150/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM150 + bool "bmm150" + default n + ---help--- + Enable support for bosch bmm150 diff --git a/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp b/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp index 00050213b46c..50ad12df3ac8 100644 --- a/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp +++ b/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -36,31 +36,13 @@ #include #include -I2CSPIDriverBase *BMM150::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - BMM150 *instance = new BMM150(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void BMM150::print_usage() { PRINT_MODULE_USAGE_NAME("bmm150", "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_ADDRESS(0x10); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -71,16 +53,17 @@ extern "C" int bmm150_main(int argc, char *argv[]) using ThisDriver = BMM150; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp index 42cdf7708a2f..7263c96783ef 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp @@ -33,9 +33,9 @@ #include "HMC5883.hpp" -HMC5883::HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), rotation), +HMC5883::HMC5883(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _px4_mag(interface->get_device_id(), config.rotation), _interface(interface), _range_ga(1.9f), _collect_phase(false), @@ -48,7 +48,6 @@ HMC5883::HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOpt _temperature_counter(0), _temperature_error_count(0) { - _px4_mag.set_external(_interface->external()); } HMC5883::~HMC5883() @@ -367,7 +366,7 @@ int HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - if (!_px4_mag.external()) { + if (!_interface->external()) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.hpp b/src/drivers/magnetometer/hmc5883/HMC5883.hpp index d5dc45ce3724..9192fb5565dd 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.hpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.hpp @@ -86,11 +86,10 @@ class HMC5883 : public I2CSPIDriver { public: - HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); + HMC5883(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~HMC5883(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/hmc5883/Kconfig b/src/drivers/magnetometer/hmc5883/Kconfig new file mode 100644 index 000000000000..658ab4b86353 --- /dev/null +++ b/src/drivers/magnetometer/hmc5883/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_HMC5883 + bool "hmc5883" + default n + ---help--- + Enable support for hmc5883 \ No newline at end of file diff --git a/src/drivers/magnetometer/hmc5883/hmc5883.h b/src/drivers/magnetometer/hmc5883/hmc5883.h index 2b46e753a1be..c4ffa91d07bc 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883.h +++ b/src/drivers/magnetometer/hmc5883/hmc5883.h @@ -42,6 +42,8 @@ #include #include +#define HMC5883L_ADDRESS 0x1E + #define ADDR_ID_A 0x0a #define ADDR_ID_B 0x0b #define ADDR_ID_C 0x0c diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp index 628263d163b1..99ba88bebff0 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp @@ -42,8 +42,6 @@ #include "hmc5883.h" -#define HMC5883L_ADDRESS 0x1E - device::Device *HMC5883_I2C_interface(int bus, int bus_frequency); class HMC5883_I2C : public device::I2C @@ -74,7 +72,7 @@ int HMC5883_I2C::probe() { uint8_t data[3] = {0, 0, 0}; - _retries = 10; + _retries = 1; if (read(ADDR_ID_A, &data[0], 1) || read(ADDR_ID_B, &data[1], 1) || @@ -83,8 +81,6 @@ int HMC5883_I2C::probe() return -EIO; } - _retries = 2; - if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp index 52b30a523e1c..a8fa83f4f60a 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015, 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 @@ -45,16 +45,15 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -I2CSPIDriverBase *HMC5883::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *HMC5883::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = HMC5883_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = HMC5883_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = HMC5883_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = HMC5883_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -64,11 +63,11 @@ I2CSPIDriverBase *HMC5883::instantiate(const BusCLIArguments &cli, const BusInst if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); return nullptr; } - HMC5883 *dev = new HMC5883(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); + HMC5883 *dev = new HMC5883(interface, config); if (dev == nullptr) { delete interface; @@ -80,7 +79,7 @@ I2CSPIDriverBase *HMC5883::instantiate(const BusCLIArguments &cli, const BusInst return nullptr; } - bool enable_temp_compensation = cli.custom1 == 1; + bool enable_temp_compensation = config.custom1 == 1; if (enable_temp_compensation) { dev->set_temperature_compensation(1); @@ -108,10 +107,10 @@ extern "C" int hmc5883_main(int argc, char *argv[]) cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:T")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:T")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; case 'T': @@ -120,13 +119,15 @@ extern "C" int hmc5883_main(int argc, char *argv[]) } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); return -1; } + cli.i2c_address = HMC5883L_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_HMC5883); if (!strcmp(verb, "start")) { diff --git a/src/drivers/magnetometer/isentek/Kconfig b/src/drivers/magnetometer/isentek/Kconfig new file mode 100644 index 000000000000..a30e5f58ff92 --- /dev/null +++ b/src/drivers/magnetometer/isentek/Kconfig @@ -0,0 +1,3 @@ +menu "Isentek" +rsource "*/Kconfig" +endmenu #Isentek diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index 1dcedf31cf1d..73507df07496 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -IST8308::IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_IST8308, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +IST8308::IST8308(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } IST8308::~IST8308() @@ -86,14 +85,19 @@ void IST8308::print_status() int IST8308::probe() { - const uint8_t WAI = RegisterRead(Register::WAI); + for (int retry = 0; retry < 3; retry++) { + const uint8_t WAI = RegisterRead(Register::WAI); - if (WAI != Device_ID) { - DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); - return PX4_ERROR; + if (WAI == Device_ID) { + _retries = 1; + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); + } } - return PX4_OK; + return PX4_ERROR; } void IST8308::RunImpl() @@ -240,7 +244,7 @@ bool IST8308::Configure() } // 1 Microtesla = 0.01 Gauss - _px4_mag.set_scale(1.f / 6.6f * 0.01f); // 6.6 LSB/uT + _px4_mag.set_scale(1.f / 13.2f * 0.01f); // 13.2 LSB/uT return success; } diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp index 9d55ac59b152..1a958b8e47d8 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -53,11 +53,9 @@ using namespace iSentek_IST8308; class IST8308 : public device::I2C, public I2CSPIDriver { public: - IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + IST8308(const I2CSPIDriverConfig &config); ~IST8308() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -103,13 +101,14 @@ class IST8308 : public device::I2C, public I2CSPIDriver } _state{STATE::RESET}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{5}; + static constexpr uint8_t size_register_cfg{6}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::ACTR, 0, ACTR_BIT::SUSPEND_EN }, - { Register::CNTL1, CNTL1_BIT::NSF_Low, 0 }, - { Register::CNTL2, CNTL2_BIT::MODE_ODR_50Hz, 0 }, + { Register::CNTL1, 0, CNTL1_BIT::NSF_DISABLE }, + { Register::CNTL2, CNTL2_BIT::MODE_ODR_50HZ_SET, CNTL2_BIT::MODE_ODR_50HZ_CLEAR }, { Register::CNTL3, 0, CNTL3_BIT::SRST }, - { Register::OSRCNTL, OSRCNTL_BIT::OSR_y_32 | OSRCNTL_BIT::OSR_xz_32, 0 }, + { Register::CNTL4, CNTL4_BIT::DR_200UT_SET, CNTL4_BIT::DR_200UT_CLEAR }, + { Register::OSRCNTL, OSRCNTL_BIT::OSR_Y_32_SET | OSRCNTL_BIT::OSR_XZ_32_SET, OSRCNTL_BIT::OSR_Y_32_CLEAR | OSRCNTL_BIT::OSR_XZ_32_CLEAR }, }; }; diff --git a/src/drivers/magnetometer/isentek/ist8308/Kconfig b/src/drivers/magnetometer/isentek/ist8308/Kconfig new file mode 100644 index 000000000000..a79ed1d71b7a --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8308/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_ISENTEK_IST8308 + bool "IST8308" + default n + ---help--- + Enable support for isentek IST8308 diff --git a/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp b/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp index 1de506b83231..1910dccbbbc9 100644 --- a/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp +++ b/src/drivers/magnetometer/isentek/ist8308/iSentek_IST8308_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -96,18 +96,14 @@ enum ACTR_BIT : uint8_t { // CNTL1 enum CNTL1_BIT : uint8_t { // 6:5 NSF[1:0]: Noise Suppression Filter setting - NSF_Low = Bit5, - NSF_Middle = Bit6, - NSF_High = Bit6 | Bit5, + NSF_DISABLE = Bit6 | Bit5, // Disable }; // CNTL2 enum CNTL2_BIT : uint8_t { // 4:0 MODE [4:0]: Operation mode setting - MODE_ODR_10Hz = Bit1, // 5’h02: Continuous Measurement Mode with ODR 10Hz - MODE_ODR_20Hz = Bit2, // 5’h04: Continuous Measurement Mode with ODR 20Hz - MODE_ODR_50Hz = Bit2 | Bit1, // 5’h06: Continuous Measurement Mode with ODR 50Hz - MODE_ODR_100Hz = Bit3, // 5’h08: Continuous Measurement Mode with ODR 100Hz + MODE_ODR_50HZ_SET = Bit2 | Bit1, // 5’h06: Continuous Measurement Mode with ODR 50Hz + MODE_ODR_50HZ_CLEAR = Bit3 | Bit0, }; // CNTL3 @@ -115,15 +111,22 @@ enum CNTL3_BIT : uint8_t { SRST = Bit0, // Soft reset, perform the same routine as POR }; +// CNTL4 +enum CNTL4_BIT : uint8_t { + // DR [1:0]: Sensor Dynamic Range and Sensitivity setting + DR_200UT_SET = Bit0, // ±200 uT, Sensitivity=13.2 LSB/uT + DR_200UT_CLEAR = Bit1 +}; + // OSRCNTL enum OSRCNTL_BIT : uint8_t { // 5:3 - OSR_y_16 = Bit5, // 3’b100: OSR=16 (ODRmax=100) (Default) - OSR_y_32 = Bit5 | Bit3, // 3’b101: OSR=32 (ODRmax=50) + OSR_Y_32_SET = Bit5 | Bit3, // 3’b101: OSR=32 (ODRmax=50) + OSR_Y_32_CLEAR = Bit4, // 2:0 - OSR_xz_16 = Bit2, // 3’b100: OSR=16 (ODRmax=100) (Default) - OSR_xz_32 = Bit2 | Bit0, // 3’b101: OSR=32 (ODRmax=50) + OSR_XZ_32_SET = Bit2 | Bit0, // 3’b101: OSR=32 (ODRmax=50) + OSR_XZ_32_CLEAR = Bit1, }; } // namespace iSentek_IST8308 diff --git a/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp index 839d1f303b4c..cedf12da621d 100644 --- a/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -36,31 +36,13 @@ #include #include -I2CSPIDriverBase *IST8308::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - IST8308 *instance = new IST8308(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void IST8308::print_usage() { PRINT_MODULE_USAGE_NAME("ist8308", "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_ADDRESS(0x0c); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -71,16 +53,17 @@ extern "C" int ist8308_main(int argc, char *argv[]) using ThisDriver = IST8308; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index 4a5cf6c536a1..b7d8512ce931 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -IST8310::IST8310(I2CSPIBusOption bus_option, int bus, uint8_t addr, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_IST8310, MODULE_NAME, bus, addr, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, addr), - _px4_mag(get_device_id(), rotation) +IST8310::IST8310(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } IST8310::~IST8310() diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp index 90921ec04359..a251f49cf02d 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -53,11 +53,9 @@ using namespace iSentek_IST8310; class IST8310 : public device::I2C, public I2CSPIDriver { public: - IST8310(I2CSPIBusOption bus_option, int bus, uint8_t addr, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + IST8310(const I2CSPIDriverConfig &config); ~IST8310() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/isentek/ist8310/Kconfig b/src/drivers/magnetometer/isentek/ist8310/Kconfig new file mode 100644 index 000000000000..f5466b97660e --- /dev/null +++ b/src/drivers/magnetometer/isentek/ist8310/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_ISENTEK_IST8310 + bool "IST8310" + default n + ---help--- + Enable support for isentek IST8310 diff --git a/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp b/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp index 1f3beb9e42c9..eee1a01c1de3 100644 --- a/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/ist8310_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -36,26 +36,6 @@ #include #include -I2CSPIDriverBase *IST8310::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - IST8310 *instance = new IST8310(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, cli.bus_frequency, - cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void IST8310::print_usage() { PRINT_MODULE_USAGE_NAME("ist8310", "driver"); @@ -75,15 +55,15 @@ extern "C" int ist8310_main(int argc, char *argv[]) cli.i2c_address = I2C_ADDRESS_DEFAULT; cli.default_i2c_frequency = I2C_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/lis2mdl/Kconfig b/src/drivers/magnetometer/lis2mdl/Kconfig new file mode 100644 index 000000000000..2b841d2c65ce --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_LIS2MDL + bool "lis2mdl" + default n + ---help--- + Enable support for lis2mdl \ No newline at end of file diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp index 314371e472ad..52858158a817 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp @@ -42,9 +42,9 @@ #include #include "lis2mdl.h" -LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), rotation), +LIS2MDL::LIS2MDL(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _px4_mag(interface->get_device_id(), config.rotation), _interface(interface), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), @@ -52,7 +52,6 @@ LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOpt _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _measure_interval(0) { - _px4_mag.set_external(_interface->external()); _px4_mag.set_scale(0.0015f); /* 49.152f / (2^15) */ } diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.h b/src/drivers/magnetometer/lis2mdl/lis2mdl.h index fda1cb9c9425..197739b2ca94 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl.h +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.h @@ -83,15 +83,15 @@ extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency); +#define LIS2MDLL_ADDRESS 0x1e class LIS2MDL : public I2CSPIDriver { public: - LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); + LIS2MDL(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~LIS2MDL(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual int init(); diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp index fff91803783d..6bbfe0dede46 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -53,8 +53,6 @@ #include "board_config.h" #include "lis2mdl.h" -#define LIS2MDLL_ADDRESS 0x1e - class LIS2MDL_I2C : public device::I2C { public: @@ -88,15 +86,13 @@ LIS2MDL_I2C::probe() { uint8_t data = 0; - _retries = 10; + _retries = 1; if (read(ADDR_WHO_AM_I, &data, 1)) { DEVICE_DEBUG("read_reg fail"); return -EIO; } - _retries = 2; - if (data != ID_WHO_AM_I) { DEVICE_DEBUG("LIS2MDL bad ID: %02x", data); return -EIO; diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp index f8a05eb8b767..11f8c8f3d42d 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -41,16 +41,15 @@ #include #include -I2CSPIDriverBase *LIS2MDL::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = LIS2MDL_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = LIS2MDL_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -60,11 +59,11 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const BusCLIArguments &cli, const BusInst if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); return nullptr; } - LIS2MDL *dev = new LIS2MDL(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); + LIS2MDL *dev = new LIS2MDL(interface, config); if (dev == nullptr) { delete interface; @@ -85,6 +84,7 @@ void LIS2MDL::print_usage() PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -97,21 +97,23 @@ extern "C" int lis2mdl_main(int argc, char *argv[]) cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); return -1; } + cli.i2c_address = LIS2MDLL_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL); if (!strcmp(verb, "start")) { diff --git a/src/drivers/magnetometer/lis3mdl/Kconfig b/src/drivers/magnetometer/lis3mdl/Kconfig new file mode 100644 index 000000000000..64b3b5eee38e --- /dev/null +++ b/src/drivers/magnetometer/lis3mdl/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_LIS3MDL + bool "lis3mdl" + default n + ---help--- + Enable support for lis3mdl \ No newline at end of file diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp index e82f1d664b00..5e1962f716f9 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.cpp @@ -42,9 +42,9 @@ #include #include "lis3mdl.h" -LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), rotation), +LIS3MDL::LIS3MDL(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _px4_mag(interface->get_device_id(), config.rotation), _interface(interface), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), @@ -65,7 +65,6 @@ LIS3MDL::LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOpt _temperature_counter(0), _temperature_error_count(0) { - _px4_mag.set_external(_interface->external()); } LIS3MDL::~LIS3MDL() diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index 6d698620f878..f36d99972d56 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -92,14 +92,16 @@ enum OPERATING_MODE { SINGLE }; +#define LIS3MDLL_ADDRESS 0x1e + + class LIS3MDL : public I2CSPIDriver { public: - LIS3MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); + LIS3MDL(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~LIS3MDL(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); void custom_method(const BusCLIArguments &cli) override; diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index 0540d7d8f14b..c85c03b75244 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -53,8 +53,6 @@ #include "board_config.h" #include "lis3mdl.h" -#define LIS3MDLL_ADDRESS 0x1e - class LIS3MDL_I2C : public device::I2C { public: @@ -87,20 +85,18 @@ int LIS3MDL_I2C::probe() { uint8_t data = 0; - _retries = 10; - if (read(ADDR_WHO_AM_I, &data, 1)) { DEVICE_DEBUG("read_reg fail"); return -EIO; } - _retries = 2; - if (data != ID_WHO_AM_I) { DEVICE_DEBUG("LIS3MDL bad ID: %02x", data); return -EIO; } + _retries = 1; + return OK; } diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp index 6c220ebc09f6..edd1f280f3c1 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015, 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 @@ -41,16 +41,15 @@ #include #include -I2CSPIDriverBase *LIS3MDL::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = LIS3MDL_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = LIS3MDL_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -60,11 +59,11 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const BusCLIArguments &cli, const BusInst if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); return nullptr; } - LIS3MDL *dev = new LIS3MDL(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); + LIS3MDL *dev = new LIS3MDL(interface, config); if (dev == nullptr) { delete interface; @@ -104,21 +103,23 @@ extern "C" int lis3mdl_main(int argc, char *argv[]) cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 11 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); return -1; } + cli.i2c_address = LIS3MDLL_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL); if (!strcmp(verb, "start")) { diff --git a/src/drivers/magnetometer/lsm303agr/Kconfig b/src/drivers/magnetometer/lsm303agr/Kconfig new file mode 100644 index 000000000000..a51b500c31eb --- /dev/null +++ b/src/drivers/magnetometer/lsm303agr/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_LSM303AGR + bool "lsm303agr" + default n + ---help--- + Enable support for lsm303agr \ No newline at end of file diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index f952c1117122..1818573b0737 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include /* SPI protocol address bits */ #define DIR_READ (1<<7) @@ -61,17 +61,14 @@ static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40; */ #define LSM303AGR_TIMER_REDUCTION 200 -LSM303AGR::LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_MAG_DEVTYPE_LSM303AGR, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation), +LSM303AGR::LSM303AGR(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag_read")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), _bad_values(perf_alloc(PC_COUNT, MODULE_NAME": bad_val")) { - _px4_mag.set_external(external()); - _px4_mag.set_scale(1.5f / 1000.f); // 1.5 milligauss/LSB } diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index 79b228c8ab5f..1a5a5bf5f0f3 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -85,12 +85,9 @@ static constexpr uint8_t OUTZ_H_REG_M = 0x6D; class LSM303AGR : public device::SPI, public I2CSPIDriver { public: - LSM303AGR(I2CSPIBusOption bus_option, int bus, int device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + LSM303AGR(const I2CSPIDriverConfig &config); virtual ~LSM303AGR(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; diff --git a/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp b/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp index ca999187028b..b3363dfee320 100644 --- a/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp +++ b/src/drivers/magnetometer/lsm303agr/lsm303agr_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -46,25 +46,6 @@ LSM303AGR::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LSM303AGR::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LSM303AGR *instance = new LSM303AGR(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" int lsm303agr_main(int argc, char *argv[]) { int ch; @@ -72,15 +53,15 @@ extern "C" int lsm303agr_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = 8 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/Kconfig b/src/drivers/magnetometer/lsm9ds1_mag/Kconfig new file mode 100644 index 000000000000..45e0c7235051 --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_LSM9DS1_MAG + bool "lsm9ds1_mag" + default n + ---help--- + Enable support for lsm9ds1_mag \ No newline at end of file diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index 3e4260659e34..79c224ab2f9d 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -40,13 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_MAG_DEVTYPE_ST_LSM9DS1_M, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +LSM9DS1_MAG::LSM9DS1_MAG(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } LSM9DS1_MAG::~LSM9DS1_MAG() @@ -177,7 +175,6 @@ void LSM9DS1_MAG::RunImpl() // sensor Z is up (RHC), flip z for publication // sensor X is aligned with -X of lsm9ds1 accel/gyro x = (x == INT16_MIN) ? INT16_MAX : -x; - y = y; z = (z == INT16_MIN) ? INT16_MAX : -z; _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp index e97c7aca7bd2..6218b4d0ff63 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp @@ -53,12 +53,9 @@ using namespace ST_LSM9DS1_MAG; class LSM9DS1_MAG : public device::SPI, public I2CSPIDriver { public: - LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + LSM9DS1_MAG(const I2CSPIDriverConfig &config); ~LSM9DS1_MAG() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp index 19811bd4a4c8..f2cb5fdfa2ea 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp @@ -36,26 +36,6 @@ #include #include -I2CSPIDriverBase *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void LSM9DS1_MAG::print_usage() { PRINT_MODULE_USAGE_NAME("lsm9ds1_mag", "driver"); @@ -73,15 +53,15 @@ extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/qmc5883l/Kconfig b/src/drivers/magnetometer/qmc5883l/Kconfig new file mode 100644 index 000000000000..8b4e732ccd0c --- /dev/null +++ b/src/drivers/magnetometer/qmc5883l/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_QMC5883L + bool "qmc5883l" + default n + ---help--- + Enable support for qmc5883l \ No newline at end of file diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp index 3efc44371100..5da9accaa636 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -40,12 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -QMC5883L::QMC5883L(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_QMC5883L, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation) +QMC5883L::QMC5883L(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) { - _px4_mag.set_external(external()); } QMC5883L::~QMC5883L() diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp index a0e23e3daa53..85209720afd7 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp @@ -53,11 +53,9 @@ using namespace QST_QMC5883L; class QMC5883L : public device::I2C, public I2CSPIDriver { public: - QMC5883L(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + QMC5883L(const I2CSPIDriverConfig &config); ~QMC5883L() override; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); diff --git a/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp b/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp index 183c07c7ffc8..6757b7f9a71a 100644 --- a/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp +++ b/src/drivers/magnetometer/qmc5883l/qmc5883l_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -36,31 +36,13 @@ #include #include -I2CSPIDriverBase * -QMC5883L::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) -{ - QMC5883L *instance = new QMC5883L(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return instance; -} - void QMC5883L::print_usage() { PRINT_MODULE_USAGE_NAME("qmc5883l", "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_ADDRESS(0x0d); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -71,16 +53,17 @@ extern "C" int qmc5883l_main(int argc, char *argv[]) using ThisDriver = QMC5883L; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/magnetometer/rm3100/Kconfig b/src/drivers/magnetometer/rm3100/Kconfig new file mode 100644 index 000000000000..e7cb53408147 --- /dev/null +++ b/src/drivers/magnetometer/rm3100/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_RM3100 + bool "rm3100" + default n + ---help--- + Enable support for rm3100 \ No newline at end of file diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index d7a21e80f21a..b360c394c19e 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -41,98 +41,117 @@ #include "rm3100.h" -RM3100::RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _px4_mag(interface->get_device_id(), rotation), - _interface(interface), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), - _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), - _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _continuous_mode_set(false), - _mode(SINGLE), - _measure_interval(0), - _check_state_cnt(0) +RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _px4_mag(interface->get_device_id(), config.rotation), + _interface(interface) { - _px4_mag.set_external(_interface->external()); - _px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS)); } RM3100::~RM3100() { // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); - perf_free(_range_errors); - perf_free(_conf_errors); + perf_free(_reset_perf); + perf_free(_range_error_perf); + perf_free(_bad_transfer_perf); delete _interface; } int RM3100::self_test() { - /* Chances are that a poll event was triggered, so wait for conversion and read registers in order to clear DRDY bit */ - usleep(RM3100_CONVERSION_INTERVAL); - collect(); - - /* Fail if calibration is not good */ - int ret = 0; - uint8_t cmd = 0; + bool complete = false; - /* Configure mag into self test mode */ - cmd = BIST_SELFTEST; - ret = _interface->write(ADDR_BIST, &cmd, 1); + uint8_t cmd = (CMM_DEFAULT | POLLING_MODE); + int ret = _interface->write(ADDR_CMM, &cmd, 1); if (ret != PX4_OK) { return ret; } - /* Now we need to write to POLL to launch self test */ - cmd = POLL_XYZ; - ret = _interface->write(ADDR_POLL, &cmd, 1); + // Configure sensor to execute BIST upon receipt of a POLL command + cmd = BIST_SELFTEST; + ret = _interface->write(ADDR_BIST, &cmd, 1); if (ret != PX4_OK) { return ret; } - /* Now wait for status register */ - usleep(RM3100_CONVERSION_INTERVAL); + /* Perform test procedure until a valid result is obtained or test times out */ + const hrt_abstime t_start = hrt_absolute_time(); - if (check_measurement() != PX4_OK) { - return -1;; - } + while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) { - /* Now check BIST register to see whether self test is ok or not*/ - ret = _interface->read(ADDR_BIST, &cmd, 1); + // Re-disable DRDY clear + cmd = HSHAKE_NO_DRDY_CLEAR; + ret = _interface->write(ADDR_HSHAKE, &cmd, 1); - if (ret != PX4_OK) { - return ret; - } + if (ret != PX4_OK) { + return ret; + } - ret = !((cmd & BIST_XYZ_OK) == BIST_XYZ_OK); + // Poll for a measurement + cmd = POLL_XYZ; + ret = _interface->write(ADDR_POLL, &cmd, 1); - return ret; -} + if (ret != PX4_OK) { + return ret; + } -int RM3100::check_measurement() -{ - uint8_t status = 0; - int ret = _interface->read(ADDR_STATUS, &status, 1); + uint8_t status = 0; + ret = _interface->read(ADDR_STATUS, &status, 1); - if (ret != 0) { - return ret; + if (ret != PX4_OK) { + return ret; + } + + // If the DRDY bit in the status register is set, BIST should be complete + if (status & STATUS_DRDY) { + // Check BIST register to evaluate the test result + ret = _interface->read(ADDR_BIST, &cmd, 1); + + if (ret != PX4_OK) { + return ret; + } + + // The test results are not valid if STE is not set. In this case, we try again + if (cmd & BIST_STE) { + complete = true; + + // If the test passed, disable self-test mode by clearing the STE bit + if (cmd & BIST_XYZ_OK) { + cmd = 0; + ret = _interface->write(ADDR_BIST, &cmd, 1); + + if (ret != PX4_OK) { + PX4_ERR("Failed to disable built-in self test"); + } + + return PX4_OK; + + } else { + PX4_ERR("built-in self test failed"); + } + } + } } - return !((status & STATUS_DRDY) == STATUS_DRDY) ; + if (!complete) { + PX4_ERR("built-in self test incomplete"); + } + + return PX4_ERROR; } -int RM3100::collect() +void RM3100::RunImpl() { - /* Check whether a measurement is available or not, otherwise return immediately */ - if (check_measurement() != 0) { - PX4_DEBUG("No measurement available"); - return 0; + // full reset if things are failing consistently + if (_failure_count > 10) { + _failure_count = 0; + set_default_register_values(); + ScheduleOnInterval(RM3100_INTERVAL); + return; } struct { @@ -141,21 +160,15 @@ int RM3100::collect() uint8_t z[3]; } rm_report{}; - _px4_mag.set_error_count(perf_event_count(_comms_errors)); - - perf_begin(_sample_perf); - const hrt_abstime timestamp_sample = hrt_absolute_time(); int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report)); if (ret != OK) { - perf_end(_sample_perf); - perf_count(_comms_errors); - return ret; + perf_count(_bad_transfer_perf); + _failure_count++; + return; } - perf_end(_sample_perf); - /* Rearrange mag data */ int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]); int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]); @@ -166,12 +179,36 @@ int RM3100::collect() convert_signed(&yraw); convert_signed(&zraw); - _px4_mag.update(timestamp_sample, xraw, yraw, zraw); + // valid range: -8388608 to 8388607 + if (xraw < -8388608 || xraw > 8388607 || + yraw < -8388608 || yraw > 8388607 || + zraw < -8388608 || zraw > 8388607) { - ret = OK; + _failure_count++; + perf_count(_range_error_perf); + return; + } - return ret; + // only publish changes + if (_raw_data_prev[0] != xraw || _raw_data_prev[1] != yraw || _raw_data_prev[2] != zraw) { + + _px4_mag.set_error_count(perf_event_count(_bad_transfer_perf) + + perf_event_count(_range_error_perf)); + + _px4_mag.update(timestamp_sample, xraw, yraw, zraw); + + _raw_data_prev[0] = xraw; + _raw_data_prev[1] = yraw; + _raw_data_prev[2] = zraw; + + if (_failure_count > 0) { + _failure_count--; + } + + } else { + _failure_count++; + } } void RM3100::convert_signed(int32_t *n) @@ -182,106 +219,34 @@ void RM3100::convert_signed(int32_t *n) } } -void RM3100::RunImpl() -{ - /* _measure_interval == 0 is used as _task_should_exit */ - if (_measure_interval == 0) { - return; - } - - /* Collect last measurement at the start of every cycle */ - if (collect() != OK) { - PX4_DEBUG("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - if (measure() != OK) { - PX4_DEBUG("measure error"); - } - - if (_measure_interval > 0) { - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_measure_interval); - } -} - int RM3100::init() { - /* reset the device configuration */ - reset(); - int ret = self_test(); if (ret != PX4_OK) { PX4_ERR("self test failed"); } - _measure_interval = RM3100_CONVERSION_INTERVAL; - start(); - - return ret; -} - -int RM3100::measure() -{ - int ret = 0; - uint8_t cmd = 0; - - /* Send the command to begin a measurement. */ - if ((_mode == CONTINUOUS) && !_continuous_mode_set) { - cmd = (CMM_DEFAULT | CONTINUOUS_MODE); - ret = _interface->write(ADDR_CMM, &cmd, 1); - _continuous_mode_set = true; - - } else if (_mode == SINGLE) { - if (_continuous_mode_set) { - /* This is needed for polling mode */ - cmd = (CMM_DEFAULT | POLLING_MODE); - ret = _interface->write(ADDR_CMM, &cmd, 1); - - if (ret != OK) { - perf_count(_comms_errors); - return ret; - } - - _continuous_mode_set = false; - } - - cmd = POLL_XYZ; - ret = _interface->write(ADDR_POLL, &cmd, 1); - } - - - if (ret != OK) { - perf_count(_comms_errors); + if (set_default_register_values() == PX4_OK) { + ScheduleOnInterval(RM3100_INTERVAL); + return PX4_OK; } - return ret; + return PX4_ERROR; } void RM3100::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - PX4_INFO("poll interval: %u", _measure_interval); -} - -int RM3100::reset() -{ - int ret = set_default_register_values(); - - if (ret != OK) { - return PX4_ERROR; - } - - return PX4_OK; + perf_print_counter(_reset_perf); + perf_print_counter(_range_error_perf); + perf_print_counter(_bad_transfer_perf); } int RM3100::set_default_register_values() { + perf_count(_reset_perf); + uint8_t cmd[2] = {0, 0}; cmd[0] = CCX_DEFAULT_MSB; @@ -307,11 +272,3 @@ int RM3100::set_default_register_values() return PX4_OK; } - -void RM3100::start() -{ - set_default_register_values(); - - /* schedule a cycle to start things */ - ScheduleNow(); -} diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index ee10595ee479..7e4737d48bc4 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -50,8 +50,7 @@ * RM3100 internal constants and data structures. */ -/* At 146 Hz we encounter errors, 100 Hz is safer */ -#define RM3100_CONVERSION_INTERVAL 10000 // Microseconds, corresponds to 100 Hz (cycle count 200 on 3 axis) +#define RM3100_INTERVAL 13000 // 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95) #define UTESLA_TO_GAUSS 100.0f #define RM3100_SENSITIVITY 75.0f @@ -75,40 +74,37 @@ #define CCY_DEFAULT_LSB CCX_DEFAULT_LSB #define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB #define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB -#define CMM_DEFAULT 0x70 // No continuous mode +#define CMM_DEFAULT 0b0111'0001 // continuous mode #define CONTINUOUS_MODE (1 << 0) #define POLLING_MODE (0 << 0) -#define TMRC_DEFAULT 0x94 -#define BIST_SELFTEST 0x8F +#define TMRC_DEFAULT 0x95 // ~13 ms, ~75 Hz +#define BIST_SELFTEST 0b1000'1111 #define BIST_DEFAULT 0x00 #define BIST_XYZ_OK ((1 << 4) | (1 << 5) | (1 << 6)) +#define BIST_STE (1 << 7) +#define BIST_DUR_USEC (2*RM3100_INTERVAL) +#define HSHAKE_DEFAULT (0x0B) +#define HSHAKE_NO_DRDY_CLEAR (0x08) #define STATUS_DRDY (1 << 7) #define POLL_XYZ 0x70 -#define RM3100_REVID 0x22 -#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) +#define RM3100_REVID 0x22 /* interface factories */ extern device::Device *RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); extern device::Device *RM3100_I2C_interface(int bus, int bus_frequency); -enum OPERATING_MODE { - CONTINUOUS = 0, - SINGLE -}; +#define RM3100_ADDRESS 0x20 class RM3100 : public I2CSPIDriver { public: - RM3100(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); + RM3100(device::Device *interface, const I2CSPIDriverConfig &config); virtual ~RM3100(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); - void custom_method(const BusCLIArguments &cli) override; - int init(); void print_status() override; @@ -121,29 +117,6 @@ class RM3100 : public I2CSPIDriver void RunImpl(); private: - PX4Magnetometer _px4_mag; - - device::Device *_interface; - - perf_counter_t _comms_errors; - perf_counter_t _conf_errors; - perf_counter_t _range_errors; - perf_counter_t _sample_perf; - - /* status reporting */ - bool _continuous_mode_set; - - enum OPERATING_MODE _mode; - - unsigned int _measure_interval; - - uint8_t _check_state_cnt; - - /** - * Collect the result of the most recent measurement. - */ - int collect(); - /** * Run sensor self-test * @@ -151,36 +124,21 @@ class RM3100 : public I2CSPIDriver */ int self_test(); - /** - * Check whether new data is available or not - * - * @return 0 if new data is available, 1 else - */ - int check_measurement(); - /** * Converts int24_t stored in 32-bit container to int32_t */ void convert_signed(int32_t *n); - /** - * Issue a measurement command. - * - * @return OK if the measurement command was successful. - */ - int measure(); + PX4Magnetometer _px4_mag; - /** - * @brief Resets the device - */ - int reset(); + device::Device *_interface{nullptr}; - /** - * @brief Initialises the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _range_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": range error")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; -}; // class RM3100 + int32_t _raw_data_prev[3] {}; + + int _failure_count{0}; + +}; diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index e9691ae2ab13..fe13bebfd3d6 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -53,8 +53,6 @@ #include "board_config.h" #include "rm3100.h" -#define RM3100_ADDRESS 0x20 - class RM3100_I2C : public device::I2C { public: @@ -86,20 +84,18 @@ int RM3100_I2C::probe() { uint8_t data = 0; - _retries = 10; - if (read(ADDR_REVID, &data, 1)) { DEVICE_DEBUG("RM3100 read_reg fail"); return -EIO; } - _retries = 2; - if (data != RM3100_REVID) { DEVICE_DEBUG("RM3100 bad ID: %02x", data); return -EIO; } + _retries = 1; + return OK; } diff --git a/src/drivers/magnetometer/rm3100/rm3100_main.cpp b/src/drivers/magnetometer/rm3100/rm3100_main.cpp index 05e2e61a5a00..997c6f224367 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_main.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -41,16 +41,15 @@ #include #include -I2CSPIDriverBase *RM3100::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *RM3100::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { device::Device *interface = nullptr; - if (iterator.busType() == BOARD_I2C_BUS) { - interface = RM3100_I2C_interface(iterator.bus(), cli.bus_frequency); + if (config.bus_type == BOARD_I2C_BUS) { + interface = RM3100_I2C_interface(config.bus, config.bus_frequency); - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = RM3100_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = RM3100_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); } if (interface == nullptr) { @@ -60,11 +59,11 @@ I2CSPIDriverBase *RM3100::instantiate(const BusCLIArguments &cli, const BusInsta if (interface->init() != OK) { delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); return nullptr; } - RM3100 *dev = new RM3100(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); + RM3100 *dev = new RM3100(interface, config); if (dev == nullptr) { delete interface; @@ -79,12 +78,6 @@ I2CSPIDriverBase *RM3100::instantiate(const BusCLIArguments &cli, const BusInsta return dev; } -void -RM3100::custom_method(const BusCLIArguments &cli) -{ - reset(); -} - void RM3100::print_usage() { PRINT_MODULE_USAGE_NAME("rm3100", "driver"); @@ -92,7 +85,6 @@ void RM3100::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -104,39 +96,35 @@ extern "C" int rm3100_main(int argc, char *argv[]) cli.default_i2c_frequency = 400000; cli.default_spi_frequency = 1 * 1000 * 1000; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); return -1; } + cli.i2c_address = RM3100_ADDRESS; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_RM3100); if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "reset")) { - return ThisDriver::module_custom_method(cli, iterator); - } - ThisDriver::print_usage(); return -1; } diff --git a/src/drivers/magnetometer/vtrantech/CMakeLists.txt b/src/drivers/magnetometer/vtrantech/CMakeLists.txt new file mode 100644 index 000000000000..d3e0ca3f70d1 --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(vcm1193l) diff --git a/src/drivers/magnetometer/vtrantech/Kconfig b/src/drivers/magnetometer/vtrantech/Kconfig new file mode 100644 index 000000000000..fdfea7f0cf8f --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/Kconfig @@ -0,0 +1,3 @@ +menu "Vtrantech" +rsource "*/Kconfig" +endmenu #Vtrantech diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/CMakeLists.txt b/src/drivers/magnetometer/vtrantech/vcm1193l/CMakeLists.txt new file mode 100644 index 000000000000..58d28fc1ccd8 --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__magnetometer__vcm1193l + MAIN vcm1193l + COMPILE_FLAGS + SRCS + VCM1193L.cpp + VCM1193L.hpp + vcm1193l_main.cpp + VTT_VCM1193L_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/Kconfig b/src/drivers/magnetometer/vtrantech/vcm1193l/Kconfig new file mode 100644 index 000000000000..32025a269457 --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L + bool "VCM1193L" + default n + ---help--- + Enable support for Vtrantech VCM1193L diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp new file mode 100644 index 000000000000..1767e2b27aee --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp @@ -0,0 +1,299 @@ +/**************************************************************************** + * + * 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 "VCM1193L.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +VCM1193L::VCM1193L(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) +{ +} + +VCM1193L::~VCM1193L() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int VCM1193L::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool VCM1193L::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void VCM1193L::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int VCM1193L::probe() +{ + _retries = 1; + + for (int i = 0; i < 3; i++) { + // first read 0x0 once + const uint8_t cmd = 0; + uint8_t buffer{}; + + if (transfer(&cmd, 1, &buffer, 1) == PX4_OK) { + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + if (CHIP_ID == Chip_ID) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void VCM1193L::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CNTL1: Software Reset + RegisterWrite(Register::CNTL1, CNTL1_BIT::SOFT_RST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(10_ms); // POR Completion Time (not documented) + break; + + case STATE::WAIT_FOR_RESET: + + // SOFT_RST: This bit is automatically reset to zero after POR routine + if ((RegisterRead(Register::CHIP_ID) == Chip_ID) + && (RegisterRead(Register::CNTL1) == 0)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 20 ms (50 Hz) + _state = STATE::READ; + ScheduleOnInterval(20_ms, 20_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + struct TransferBuffer { + uint8_t X_LSB; + uint8_t X_MSB; + uint8_t Y_LSB; + uint8_t Y_MSB; + uint8_t Z_LSB; + uint8_t Z_MSB; + } buffer{}; + + bool success = false; + uint8_t cmd = static_cast(Register::X_LSB); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + int16_t x = combine(buffer.X_MSB, buffer.X_LSB); + int16_t y = combine(buffer.Y_MSB, buffer.Y_LSB); + int16_t z = combine(buffer.Z_MSB, buffer.Z_LSB); + + if (x != _prev_data[0] || y != _prev_data[1] || z != _prev_data[2]) { + _prev_data[0] = x; + _prev_data[1] = y; + _prev_data[2] = z; + + // Data Sheet is incorrect + y = (y == INT16_MIN) ? INT16_MAX : -y; // -y + + _px4_mag.update(now, x, y, z); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + 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])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +bool VCM1193L::Configure() +{ + // first set and clear all configured register bits + + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_mag.set_scale(1.f / 3000.f); // 3000 LSB/Gauss (Field Range = ±8G) + + return success; +} + +bool VCM1193L::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02" PRIX8 ": 0x%02" PRIX8 "(0x%02" PRIX8 " not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02" PRIX8 ": 0x%02" PRIX8 " (0x%02" PRIX8 " not cleared)", (uint8_t)reg_cfg.reg, reg_value, + reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t VCM1193L::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void VCM1193L::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void VCM1193L::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.hpp b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.hpp new file mode 100644 index 000000000000..c0bb0a61472b --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 VCM1193L.hpp + * + * Driver for the VCM1193L connected via I2C. + * + */ + +#pragma once + +#include "VTT_VCM1193L_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace VTT_VCM1193L; + +class VCM1193L : public device::I2C, public I2CSPIDriver +{ +public: + VCM1193L(const I2CSPIDriverConfig &config); + ~VCM1193L() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + int16_t _prev_data[3] {}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{2}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL1, 0, CNTL1_BIT::SET_RESET | CNTL1_BIT::SOFT_RST}, + { Register::CNTL2, CNTL2_BIT::ODR_50HZ | CNTL2_BIT::Mode_Normal | CNTL2_BIT::CTRL2_INIT, CNTL2_BIT::ODR2}, + }; +}; diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/VTT_VCM1193L_registers.hpp b/src/drivers/magnetometer/vtrantech/vcm1193l/VTT_VCM1193L_registers.hpp new file mode 100644 index 000000000000..71b2ee7eb87a --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/VTT_VCM1193L_registers.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * 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 VTT_VCM1193L_registers.hpp + * + * QST VCM1193L registers. + * + */ + +#pragma once + +#include + +namespace VTT_VCM1193L +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100; // default I2C address + +static constexpr uint8_t Chip_ID = 0x82; + +enum class Register : uint8_t { + X_LSB = 0x00, // Data Output X LSB Register XOUT[7:0] + X_MSB = 0x01, // Data Output X MSB Register XOUT[15:8] + Y_LSB = 0x02, // Data Output Y LSB Register YOUT[7:0] + Y_MSB = 0x03, // Data Output Y MSB Register YOUT[15:8] + Z_LSB = 0x04, // Data Output Z LSB Register ZOUT[7:0] + Z_MSB = 0x05, // Data Output Z MSB Register ZOUT[15:8] + + CNTL2 = 0x0a, // Control Register 2 + CNTL1 = 0x0b, // Control Register 1 + REG_ID = 0x0c, // Chip ID + + CHIP_ID = 0x0c, +}; + + +// CNTL2 +enum CNTL2_BIT : uint8_t { + // + // 0AH[7:4] Note : During initialization , register 0AH(bit7~bit4) must be written 0100b + CTRL2_INIT = Bit7, + + // ODR[3:2] + ODR2 = Bit2, + ODR_50HZ = Bit3, // 10 + + // MODE[0] + Mode0 = Bit0, // + Mode_Normal = Bit0, // 1 +}; + +// CNTL1 + +enum CNTL1_BIT : uint8_t { + SOFT_RST = Bit7, // Soft reset, restore default value of all registers. + SET_RESET = Bit1 | Bit0, // Set Reset (Cleared) +}; + +} // namespace VCM1193L diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/vcm1193l_main.cpp b/src/drivers/magnetometer/vtrantech/vcm1193l/vcm1193l_main.cpp new file mode 100644 index 000000000000..9706cbfe0e7b --- /dev/null +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/vcm1193l_main.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * 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 "VCM1193L.hpp" + +void VCM1193L::print_usage() +{ + PRINT_MODULE_USAGE_NAME("vcm1193l", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int vcm1193l_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = VCM1193L; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_VCM1193L); + + 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/optical_flow/CMakeLists.txt b/src/drivers/optical_flow/CMakeLists.txt deleted file mode 100644 index ab6c47729013..000000000000 --- a/src/drivers/optical_flow/CMakeLists.txt +++ /dev/null @@ -1,37 +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. -# -############################################################################ - -add_subdirectory(paw3902) -add_subdirectory(pmw3901) -add_subdirectory(px4flow) -add_subdirectory(thoneflow) diff --git a/src/drivers/optical_flow/Kconfig b/src/drivers/optical_flow/Kconfig new file mode 100644 index 000000000000..7926d5039b56 --- /dev/null +++ b/src/drivers/optical_flow/Kconfig @@ -0,0 +1,13 @@ +menu "Optical flow" + menuconfig COMMON_OPTICAL_FLOW + bool "Common Optical flow" + default n + select DRIVERS_OPTICAL_FLOW_PAA3905 + select DRIVERS_OPTICAL_FLOW_PAW3902 + select DRIVERS_OPTICAL_FLOW_PMW3901 + select DRIVERS_OPTICAL_FLOW_PX4FLOW + select DRIVERS_OPTICAL_FLOW_THONEFLOW + ---help--- + Enable default set of magnetometer drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/optical_flow/paa3905/CMakeLists.txt b/src/drivers/optical_flow/paa3905/CMakeLists.txt new file mode 100644 index 000000000000..0e699c9b81f5 --- /dev/null +++ b/src/drivers/optical_flow/paa3905/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__optical_flow__paa3905 + MAIN paa3905 + SRCS + paa3905_main.cpp + PAA3905.cpp + PAA3905.hpp + PixArt_PAA3905_Registers.hpp + DEPENDS + conversion + drivers__device + px4_work_queue + ) diff --git a/src/drivers/optical_flow/paa3905/Kconfig b/src/drivers/optical_flow/paa3905/Kconfig new file mode 100644 index 000000000000..927ebd2c94f8 --- /dev/null +++ b/src/drivers/optical_flow/paa3905/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OPTICAL_FLOW_PAA3905 + bool "paa3905" + default n + ---help--- + Enable support for paa3905 diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp new file mode 100644 index 000000000000..cd87391c8402 --- /dev/null +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -0,0 +1,640 @@ +/**************************************************************************** + * + * 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 "PAA3905.hpp" + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +PAA3905::PAA3905(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) +{ + if (_drdy_gpio != 0) { + _no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt"); + } + + float yaw_rotation_degrees = (float)config.custom1; + + if (yaw_rotation_degrees >= 0.f) { + PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)", + (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees)); + + _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; + + } else { + _rotation.identity(); + } +} + +PAA3905::~PAA3905() +{ + // free perf counters + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_reset_perf); + perf_free(_false_motion_perf); + perf_free(_mode_change_bright_perf); + perf_free(_mode_change_low_light_perf); + perf_free(_mode_change_super_low_light_perf); + perf_free(_no_motion_interrupt_perf); +} + +int PAA3905::init() +{ + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + return PX4_ERROR; + } + + Configure(); + + return PX4_OK; +} + +int PAA3905::probe() +{ + for (int retry = 0; retry < 3; retry++) { + const uint8_t Product_ID = RegisterRead(Register::Product_ID); + const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); + + if (Product_ID != PRODUCT_ID) { + PX4_ERR("Product_ID: %X", Product_ID); + break; + } + + if (Revision_ID != REVISION_ID) { + PX4_ERR("Revision_ID: %X", Revision_ID); + break; + } + + if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { + PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); + break; + } + + return PX4_OK; + } + + return PX4_ERROR; +} + +int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void PAA3905::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool PAA3905::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; + return false; + } + + // Setup data ready on falling edge + if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) { + _data_ready_interrupt_enabled = true; + return true; + } + + _data_ready_interrupt_enabled = false; + return false; +} + +bool PAA3905::DataReadyInterruptDisable() +{ + _data_ready_interrupt_enabled = false; + + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +void PAA3905::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void PAA3905::Reset() +{ + perf_count(_reset_perf); + + DataReadyInterruptDisable(); + ScheduleClear(); + + // Issue a soft reset + RegisterWrite(Register::Power_Up_Reset, 0x5A); + px4_usleep(1000); + _last_reset = hrt_absolute_time(); + + _discard_reading = 3; + + // Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state. + RegisterRead(0x02); + RegisterRead(0x03); + RegisterRead(0x04); + RegisterRead(0x05); + RegisterRead(0x06); +} + +void PAA3905::Configure() +{ + Reset(); + + ConfigureStandardDetectionSetting(); + + ConfigureAutomaticModeSwitching(); + + EnableLed(); + + // Read Register 0x15. Check Bit [7:6] for AMS mode + const uint8_t Observation = RegisterRead(Register::Observation); + UpdateMode(Observation); + + if (DataReadyInterruptConfigure()) { + // backup schedule + ScheduleDelayed(500_ms); + + } else { + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); + } +} + +void PAA3905::ConfigureStandardDetectionSetting() +{ + // Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect + // when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is + // compromised. + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x3E); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x08); + RegisterWrite(0x71, 0x04); + RegisterWrite(0x72, 0x06); + RegisterWrite(0x74, 0x3C); + RegisterWrite(0x75, 0x28); + RegisterWrite(0x76, 0x20); + RegisterWrite(0x4E, 0xBF); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x14); + RegisterWrite(0x65, 0x0A); + RegisterWrite(0x66, 0x10); + RegisterWrite(0x55, 0x3C); + RegisterWrite(0x56, 0x28); + RegisterWrite(0x57, 0x20); + RegisterWrite(0x4A, 0x2D); + + RegisterWrite(0x4B, 0x2D); + RegisterWrite(0x4E, 0x4B); + RegisterWrite(0x69, 0xFA); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x1F); + RegisterWrite(0x47, 0x1F); + RegisterWrite(0x48, 0x0C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0x0F); + RegisterWrite(0x4A, 0x0F); + RegisterWrite(0x42, 0x02); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x76); + RegisterWrite(0x68, 0x76); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x18); + RegisterWrite(0x4B, 0x0C); + RegisterWrite(0x4C, 0x0C); + RegisterWrite(0x4D, 0x0C); + RegisterWrite(0x46, 0x0A); + RegisterWrite(0x59, 0xCD); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); +} + +void PAA3905::ConfigureEnhancedDetectionMode() +{ + // Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion + // detection is required, and also where more sensitive challenging surface detection is required. The recommended + // operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of + // sensitivity. + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x26); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x65); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x00); + RegisterWrite(0x71, 0x00); + RegisterWrite(0x72, 0x00); + RegisterWrite(0x74, 0x14); + RegisterWrite(0x75, 0x14); + RegisterWrite(0x76, 0x06); + RegisterWrite(0x4E, 0x8F); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x00); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x66, 0x00); + RegisterWrite(0x55, 0x14); + RegisterWrite(0x56, 0x14); + RegisterWrite(0x57, 0x06); + RegisterWrite(0x4A, 0x20); + + RegisterWrite(0x4B, 0x20); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x69, 0xFE); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x14); + RegisterWrite(0x47, 0x14); + RegisterWrite(0x48, 0x1C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0xE5); + RegisterWrite(0x4A, 0x05); + RegisterWrite(0x42, 0x04); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x50); + RegisterWrite(0x68, 0x50); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x1E); + RegisterWrite(0x4B, 0x1E); + RegisterWrite(0x4C, 0x34); + RegisterWrite(0x4D, 0x34); + RegisterWrite(0x46, 0x32); + RegisterWrite(0x59, 0x0D); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); +} + +void PAA3905::ConfigureAutomaticModeSwitching() +{ + // Automatic switching between Mode 0, 1 and 2: + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x68, 0x02); + RegisterWrite(0x7F, 0x00); + + // TODO: for mode 0 and 1 only + // Automatic switching between Mode 0 and 1 only: + // RegisterWrite(0x7F, 0x08); + // RegisterWrite(0x68, 0x01); // different than mode 0,1,2 + // RegisterWrite(0x7F, 0x00); +} + +void PAA3905::EnableLed() +{ + // Enable LED_N controls + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x6F, 0x0C); + RegisterWrite(0x7F, 0x00); +} + +bool PAA3905::UpdateMode(const uint8_t observation) +{ + bool mode_changed = false; + + // Bit [7:6] AMS mode + const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5; + + if (ams_mode == 0x0) { + // Mode 0 (Bright) + if (_mode != Mode::Bright) { + mode_changed = true; + perf_count(_mode_change_bright_perf); + } + + _mode = Mode::Bright; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; + + } else if (ams_mode == 0x1) { + // Mode 1 (LowLight) + if (_mode != Mode::LowLight) { + mode_changed = true; + perf_count(_mode_change_low_light_perf); + } + + _mode = Mode::LowLight; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; + + } else if (ams_mode == 0x2) { + // Mode 2 (SuperLowLight) + if (_mode != Mode::SuperLowLight) { + mode_changed = true; + perf_count(_mode_change_super_low_light_perf); + } + + _mode = Mode::SuperLowLight; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; + } + + return mode_changed; +} + +uint8_t PAA3905::RegisterRead(uint8_t reg) +{ + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); + } + + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); + + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); + } + + uint8_t cmd[2]; + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + hrt_store_absolute_time(&_last_read_time); + + return cmd[1]; +} + +void PAA3905::RegisterWrite(uint8_t reg, uint8_t data) +{ + // tSWW SPI Time Between Write Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWW_us) { + px4_udelay(TIME_TSWW_us - elapsed_last_write); + } + + uint8_t cmd[2]; + cmd[0] = DIR_WRITE(reg); + cmd[1] = data; + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); +} + +void PAA3905::RunImpl() +{ + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + const hrt_abstime now = hrt_absolute_time(); + + // force reconfigure if we haven't received valid data for quite some time + if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) { + Configure(); + perf_end(_cycle_perf); + return; + } + + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if (now < drdy_timestamp_sample + _scheduled_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_no_motion_interrupt_perf); + } + + // push backup schedule back + ScheduleDelayed(500_ms); + } + + struct TransferBuffer { + uint8_t cmd = Register::Motion_Burst; + BURST_TRANSFER data{}; + } buf{}; + static_assert(sizeof(buf) == (14 + 1)); + + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); + return; + } + + hrt_store_absolute_time(&_last_read_time); + + if (_discard_reading > 0) { + _discard_reading--; + perf_end(_cycle_perf); + return; + } + + // Bit [5:0] check if chip is working correctly + // 0x3F: chip is working correctly + if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) { + // Other value: recommend to issue a software reset + Configure(); + perf_end(_cycle_perf); + return; + } + + if (UpdateMode(buf.data.Observation)) { + // update scheduling if mode changed + if (!_data_ready_interrupt_enabled) { + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); + } + } + + // check SQUAL & Shutter values + // To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition + // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x00FF80 + // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x00FF80 + // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998 + + // 23-bit Shutter register + const uint8_t Shutter_Lower = buf.data.Shutter_Lower; + const uint8_t Shutter_Middle = buf.data.Shutter_Middle; + const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0); + + const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower; + + // Motion since last report and Surface quality non-zero + const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred; + + // Number of Features = SQUAL * 4 + bool data_valid = (buf.data.SQUAL > 0); + + switch (_mode) { + case Mode::Bright: + + // quality < 25 (0x19) and shutter >= 0x00FF80 + if ((buf.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) { + // false motion report, discarding + perf_count(_false_motion_perf); + data_valid = false; + } + + break; + + case Mode::LowLight: + + // quality < 70 (0x46) and shutter >= 0x00FF80 + if ((buf.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) { + // false motion report, discarding + perf_count(_false_motion_perf); + data_valid = false; + } + + break; + + case Mode::SuperLowLight: + + // quality < 85 (0x55) and shutter >= 0x025998 + if ((buf.data.SQUAL < 0x55) && (shutter >= 0x025998)) { + // false motion report, discarding + perf_count(_false_motion_perf); + data_valid = false; + } + + break; + } + + if (data_valid) { + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); + + report.integration_timespan_us = _scheduled_interval_us; + report.quality = buf.data.SQUAL; + + // set specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity + + switch (_mode) { + case Mode::Bright: + report.mode = sensor_optical_flow_s::MODE_BRIGHT; + break; + + case Mode::LowLight: + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + break; + + case Mode::SuperLowLight: + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; + break; + } + + if (motion_detected) { + // only populate flow if data valid (motion and quality > 0) + const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); + const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); + + // rotate measurements in yaw from sensor frame to body frame + const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; + + // datasheet provides 11.914 CPI (count per inch) scaling per meter of height + static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface) + static constexpr float INCHES_PER_METER = 39.3701f; + + // CPI/m -> radians + static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER); + + report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE; + report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; + } + + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); + + if (report.quality >= 1) { + _last_good_data = report.timestamp_sample; + } + } + + perf_end(_cycle_perf); +} + +void PAA3905::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_reset_perf); + perf_print_counter(_false_motion_perf); + perf_print_counter(_mode_change_bright_perf); + perf_print_counter(_mode_change_low_light_perf); + perf_print_counter(_mode_change_super_low_light_perf); + perf_print_counter(_no_motion_interrupt_perf); +} diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp new file mode 100644 index 000000000000..337ed19659cc --- /dev/null +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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 PAA3905.hpp + * + * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip + */ + +#pragma once + +#include "PixArt_PAA3905_Registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; +using namespace PixArt_PAA3905; + +#define DIR_WRITE(a) ((a) | Bit7) +#define DIR_READ(a) ((a) & 0x7F) + +class PAA3905 : public device::SPI, public I2CSPIDriver +{ +public: + PAA3905(const I2CSPIDriverConfig &config); + virtual ~PAA3905(); + + static void print_usage(); + + int init() override; + + void print_status() override; + + void RunImpl(); + +private: + void exit_and_cleanup() override; + + int probe() override; + + void Reset(); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + uint8_t RegisterRead(uint8_t reg); + void RegisterWrite(uint8_t reg, uint8_t data); + + void Configure(); + + void ConfigureAutomaticModeSwitching(); + + void ConfigureModeBright(); + void ConfigureModeLowLight(); + void ConfigureModeSuperLowLight(); + + void ConfigureStandardDetectionSetting(); + void ConfigureEnhancedDetectionMode(); + + void EnableLed(); + + bool UpdateMode(const uint8_t observation); + + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; + perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; + perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; + perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; + perf_counter_t _no_motion_interrupt_perf{nullptr}; + + const spi_drdy_gpio_t _drdy_gpio; + + matrix::Dcmf _rotation; + + int _discard_reading{3}; + + Mode _mode{Mode::LowLight}; + + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + hrt_abstime _last_write_time{0}; + hrt_abstime _last_read_time{0}; + + // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) + static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s; + + hrt_abstime _last_good_data{0}; + hrt_abstime _last_reset{0}; +}; diff --git a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp new file mode 100644 index 000000000000..de67e0cf4eaf --- /dev/null +++ b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace PixArt_PAA3905 +{ + +static constexpr uint8_t PRODUCT_ID = 0xA2; +static constexpr uint8_t REVISION_ID = 0x00; +static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D; + +static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps + +static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface + +// Various time delays +static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us) +static constexpr uint32_t TIME_TSWR_us = 6; // SPI Time Between Write and Read Commands +static constexpr uint32_t TIME_TSRW_TSRR_us = 2; // SPI Time Between Read And Subsequent Commands (actually 1.5us) +static constexpr uint32_t TIME_TSRAD_us = 2; // SPI Read Address-Data Delay + +enum Register : uint8_t { + Product_ID = 0x00, + Revision_ID = 0x01, + Motion = 0x02, + Delta_X_L = 0x03, + Delta_X_H = 0x04, + Delta_Y_L = 0x05, + Delta_Y_H = 0x06, + Squal = 0x07, + RawData_Sum = 0x08, + Maximum_RawData = 0x09, + Minimum_RawData = 0x0A, + Shutter_Lower = 0x0B, + Shutter_Middle = 0x0C, + Shutter_Upper = 0x0D, + + Observation = 0x15, + Motion_Burst = 0x16, + + Power_Up_Reset = 0x3A, + Shutdown = 0x3B, + + Resolution = 0x4E, + + Inverse_Product_ID = 0x5F, +}; + +enum Motion_Bit : uint8_t { + MotionOccurred = Bit7, // Motion since last report + + ChallengingSurface = Bit0, // Challenging surface is detected +}; + +enum Observation_Bit : uint8_t { + // Bit [7:6] + AMS_mode_0 = 0, + AMS_mode_1 = Bit6, + AMS_mode_2 = Bit7, + + // Bit [5:0] + WorkingCorrectly = 0x3F, +}; + +enum class Mode { + Bright = 0, + LowLight = 1, + SuperLowLight = 2, +}; + +struct BURST_TRANSFER { + uint8_t Motion; + uint8_t Observation; + uint8_t Delta_X_L; + uint8_t Delta_X_H; + uint8_t Delta_Y_L; + uint8_t Delta_Y_H; + uint8_t Reserved; + uint8_t SQUAL; + uint8_t RawData_Sum; + uint8_t Maximum_RawData; + uint8_t Minimum_RawData; + uint8_t Shutter_Upper; + uint8_t Shutter_Middle; + uint8_t Shutter_Lower; +}; + +} diff --git a/src/drivers/optical_flow/paa3905/paa3905_main.cpp b/src/drivers/optical_flow/paa3905/paa3905_main.cpp new file mode 100644 index 000000000000..ca43b70b7b5e --- /dev/null +++ b/src/drivers/optical_flow/paa3905/paa3905_main.cpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 "PAA3905.hpp" +#include + +void PAA3905::print_usage() +{ + PRINT_MODULE_USAGE_NAME("paa3905", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" __EXPORT int paa3905_main(int argc, char *argv[]) +{ + int ch = 0; + using ThisDriver = PAA3905; + BusCLIArguments cli{false, true}; + cli.custom1 = -1; + cli.spi_mode = SPIDEV_MODE3; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) { + switch (ch) { + case 'Y': + cli.custom1 = atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAA3905); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/optical_flow/paa3905/parameters.c b/src/drivers/optical_flow/paa3905/parameters.c new file mode 100644 index 000000000000..2646721d9428 --- /dev/null +++ b/src/drivers/optical_flow/paa3905/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * PAA3905 Optical Flow + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_PAA3905, 0); diff --git a/src/drivers/optical_flow/paw3902/CMakeLists.txt b/src/drivers/optical_flow/paw3902/CMakeLists.txt index 1683937e5cfe..2bd4380bf907 100644 --- a/src/drivers/optical_flow/paw3902/CMakeLists.txt +++ b/src/drivers/optical_flow/paw3902/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 @@ -37,4 +37,10 @@ px4_add_module( SRCS paw3902_main.cpp PAW3902.cpp + PAW3902.hpp + PixArt_PAW3902_Registers.hpp + DEPENDS + conversion + drivers__device + px4_work_queue ) diff --git a/src/drivers/optical_flow/paw3902/Kconfig b/src/drivers/optical_flow/paw3902/Kconfig new file mode 100644 index 000000000000..c3a8f3a82f1c --- /dev/null +++ b/src/drivers/optical_flow/paw3902/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OPTICAL_FLOW_PAW3902 + bool "paw3902" + default n + ---help--- + Enable support for paw3902 \ No newline at end of file diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index d581b9945dc5..de8e6c039cac 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -38,43 +38,39 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -PAW3902::PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode, - spi_drdy_gpio_t drdy_gpio, float yaw_rotation_degrees) : - SPI(DRV_FLOW_DEVTYPE_PAW3902, MODULE_NAME, bus, devid, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio) +PAW3902::PAW3902(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) { - if (PX4_ISFINITE(yaw_rotation_degrees)) { + if (_drdy_gpio != 0) { + _no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt"); + } + + float yaw_rotation_degrees = (float)config.custom1; + + if (yaw_rotation_degrees >= 0.f) { PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)", (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees)); _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; } else { - // otherwise use the parameter SENS_FLOW_ROT - param_t rot = param_find("SENS_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - - } else { - _rotation.identity(); - } + _rotation.identity(); } } PAW3902::~PAW3902() { // free perf counters - perf_free(_sample_perf); + perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_comms_errors); + perf_free(_reset_perf); perf_free(_false_motion_perf); - perf_free(_register_write_fail_perf); perf_free(_mode_change_bright_perf); perf_free(_mode_change_low_light_perf); perf_free(_mode_change_super_low_light_perf); + perf_free(_no_motion_interrupt_perf); } int PAW3902::init() @@ -84,40 +80,37 @@ int PAW3902::init() return PX4_ERROR; } - Reset(); - - // force to low light mode (1) initially - ChangeMode(Mode::LowLight, true); - - _previous_collect_timestamp = hrt_absolute_time(); + Configure(); return PX4_OK; } int PAW3902::probe() { - const uint8_t Product_ID = RegisterRead(Register::Product_ID); - - if (Product_ID != PRODUCT_ID) { - PX4_ERR("Product_ID: %X", Product_ID); - return PX4_ERROR; - } + for (int retry = 0; retry < 3; retry++) { + const uint8_t Product_ID = RegisterRead(Register::Product_ID); + const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + if (Product_ID != PRODUCT_ID) { + PX4_ERR("Product_ID: %X", Product_ID); + break; + } - if (Revision_ID != REVISION_ID) { - PX4_ERR("Revision_ID: %X", Revision_ID); - return PX4_ERROR; - } + if (Revision_ID != REVISION_ID) { + PX4_ERR("Revision_ID: %X", Revision_ID); + break; + } - const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); + if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { + PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); + break; + } - if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { - PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); - return PX4_ERROR; + return PX4_OK; } - return PX4_OK; + return PX4_ERROR; } int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -128,21 +121,31 @@ int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) void PAW3902::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } bool PAW3902::DataReadyInterruptConfigure() { if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; return false; } // Setup data ready on falling edge - return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; + if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) { + _data_ready_interrupt_enabled = true; + return true; + } + + _data_ready_interrupt_enabled = false; + return false; } bool PAW3902::DataReadyInterruptDisable() { + _data_ready_interrupt_enabled = false; + if (_drdy_gpio == 0) { return false; } @@ -150,431 +153,423 @@ bool PAW3902::DataReadyInterruptDisable() return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } -bool PAW3902::Reset() +void PAW3902::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void PAW3902::Reset() { + perf_count(_reset_perf); + DataReadyInterruptDisable(); + ScheduleClear(); - // Power on reset + // Issue a soft reset RegisterWrite(Register::Power_Up_Reset, 0x5A); - usleep(1000); + px4_usleep(1000); + _last_reset = hrt_absolute_time(); - // Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion state - RegisterRead(Register::Motion); - RegisterRead(Register::Delta_X_L); - RegisterRead(Register::Delta_X_H); - RegisterRead(Register::Delta_Y_L); - RegisterRead(Register::Delta_Y_H); + _discard_reading = 3; - return true; + _bright_to_low_counter = 0; + _low_to_superlow_counter = 0; + _low_to_bright_counter = 0; + _superlow_to_low_counter = 0; + + // Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state. + RegisterRead(0x02); + RegisterRead(0x03); + RegisterRead(0x04); + RegisterRead(0x05); + RegisterRead(0x06); } -void PAW3902::exit_and_cleanup() +void PAW3902::Configure() { - DataReadyInterruptDisable(); - I2CSPIDriverBase::exit_and_cleanup(); + Reset(); + + ChangeMode(Mode::LowLight, true); } bool PAW3902::ChangeMode(Mode newMode, bool force) { if (newMode != _mode || force) { PX4_DEBUG("changing from mode %d -> %d", static_cast(_mode), static_cast(newMode)); - DataReadyInterruptDisable(); - ScheduleClear(); - - // Issue a soft reset - RegisterWrite(Register::Power_Up_Reset, 0x5A); - uint32_t interval_us = 0; + Reset(); switch (newMode) { case Mode::Bright: - ModeBright(); - interval_us = SAMPLE_INTERVAL_MODE_0; + ConfigureModeBright(); + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; perf_count(_mode_change_bright_perf); break; case Mode::LowLight: - ModeLowLight(); - interval_us = SAMPLE_INTERVAL_MODE_1; + ConfigureModeLowLight(); + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; perf_count(_mode_change_low_light_perf); break; case Mode::SuperLowLight: - ModeSuperLowLight(); - interval_us = SAMPLE_INTERVAL_MODE_2; + ConfigureModeSuperLowLight(); + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; perf_count(_mode_change_super_low_light_perf); break; } + EnableLed(); + if (DataReadyInterruptConfigure()) { - // backup schedule as a watchdog timeout + // backup schedule ScheduleDelayed(500_ms); } else { - ScheduleOnInterval(interval_us); + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } - // Discard the first three motion data. - for (int i = 0; i < 3; i++) { - RegisterRead(Register::Motion); - RegisterRead(Register::Delta_X_L); - RegisterRead(Register::Delta_X_H); - RegisterRead(Register::Delta_Y_L); - RegisterRead(Register::Delta_Y_H); - } - - EnableLed(); - _mode = newMode; } - _bright_to_low_counter = 0; - _low_to_superlow_counter = 0; - _low_to_bright_counter = 0; - _superlow_to_low_counter = 0; - // Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19 - // The maximum register value is 0xA8. The minimum register value is 0. - uint8_t resolution = RegisterRead(Register::Resolution); - PX4_DEBUG("Resolution: %X", resolution); - PX4_DEBUG("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f))); - return true; } -void PAW3902::ModeBright() +void PAW3902::ConfigureModeBright() { // Mode 0: Bright (126 fps) 60 Lux typical // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); - - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x32); - RegisterWriteVerified(0x6d, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x47); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); - - usleep(10_ms); // delay 10ms - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); - - usleep(10_ms); // delay 10ms + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); + + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x32); + RegisterWrite(0x6d, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x47); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); + + px4_usleep(10'000); // delay 10ms + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); + + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeLowLight() +void PAW3902::ConfigureModeLowLight() { // Mode 1: Low Light (126 fps) 30 Lux typical // low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); - - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x65); - RegisterWriteVerified(0x6d, 0x65); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); - - usleep(10_ms); // delay 10ms - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); - - usleep(10_ms); // delay 10ms + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); + + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x65); + RegisterWrite(0x6d, 0x65); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); + + px4_usleep(10'000); // delay 10ms + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); + + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeSuperLowLight() +void PAW3902::ConfigureModeSuperLowLight() { // Mode 2: Super Low Light (50 fps) 9 Lux typical // super low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); - - RegisterWriteVerified(0x48, 0x04); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5E, 0x34); - RegisterWriteVerified(0x5B, 0x32); - RegisterWriteVerified(0x6D, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xE5); - RegisterWriteVerified(0x71, 0xE5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6A, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xCE); - RegisterWriteVerified(0x65, 0x0B); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x02); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); - - usleep(25_ms); // delay 25ms - - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x44); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x40); - RegisterWriteVerified(0x69, 0x02); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x0B); - - usleep(25_ms); // delay 25ms + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); + + RegisterWrite(0x48, 0x04); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5E, 0x34); + RegisterWrite(0x5B, 0x32); + RegisterWrite(0x6D, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xE5); + RegisterWrite(0x71, 0xE5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6A, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xCE); + RegisterWrite(0x65, 0x0B); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x02); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); + + px4_usleep(25'000); // delay 25ms + + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x44); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x40); + RegisterWrite(0x69, 0x02); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x0B); + + px4_usleep(25'000); // delay 25ms RegisterWrite(0x73, 0x00); } @@ -582,65 +577,83 @@ void PAW3902::ModeSuperLowLight() void PAW3902::EnableLed() { // Enable LED_N controls - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x6F, 0x1c); - RegisterWriteVerified(0x7F, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x6F, 0x1c); + RegisterWrite(0x7F, 0x00); } -uint8_t PAW3902::RegisterRead(uint8_t reg, int retries) +uint8_t PAW3902::RegisterRead(uint8_t reg) { - for (int i = 0; i < retries; i++) { - px4_udelay(TIME_us_TSRAD); - uint8_t cmd[2] {reg, 0}; + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); - if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { - return cmd[1]; - } + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); } - perf_count(_comms_errors); - return 0; + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); + + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); + } + + uint8_t cmd[2]; + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + hrt_store_absolute_time(&_last_read_time); + + return cmd[1]; } void PAW3902::RegisterWrite(uint8_t reg, uint8_t data) { + // tSWW SPI Time Between Write Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWW_us) { + px4_udelay(TIME_TSWW_us - elapsed_last_write); + } + uint8_t cmd[2]; cmd[0] = DIR_WRITE(reg); cmd[1] = data; - - if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) { - perf_count(_comms_errors); - } + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); } -bool PAW3902::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries) +void PAW3902::RunImpl() { - for (int i = 0; i < retries; i++) { - uint8_t cmd[2]; - cmd[0] = DIR_WRITE(reg); - cmd[1] = data; - transfer(&cmd[0], nullptr, sizeof(cmd)); - px4_udelay(TIME_us_TSWW); - - // read back to verify - uint8_t data_read = RegisterRead(reg); - - if (data_read == data) { - return true; - } + perf_begin(_cycle_perf); + perf_count(_interval_perf); - PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read); + const hrt_abstime now = hrt_absolute_time(); + + // force reconfigure if we haven't received valid data for quite some time + if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) { + PX4_ERR("no valid data for %.3f seconds, resetting", 1e-6 * (now - _last_good_data)); + Configure(); + perf_end(_cycle_perf); + return; } - perf_count(_register_write_fail_perf); + hrt_abstime timestamp_sample = now; - return false; -} + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); -void PAW3902::RunImpl() -{ - perf_begin(_sample_perf); - perf_count(_interval_perf); + if (now < drdy_timestamp_sample + _scheduled_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_no_motion_interrupt_perf); + } + + // push backup schedule back + ScheduleDelayed(500_ms); + } struct TransferBuffer { uint8_t cmd = Register::Motion_Burst; @@ -648,44 +661,53 @@ void PAW3902::RunImpl() } buf{}; static_assert(sizeof(buf) == (12 + 1)); - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); return; } - perf_end(_sample_perf); - - const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp; + hrt_store_absolute_time(&_last_read_time); - // update for next iteration - _previous_collect_timestamp = timestamp_sample; + if (_discard_reading > 0) { + _discard_reading--; + perf_end(_cycle_perf); + return; + } // check SQUAL & Shutter values // To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition - // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0 - // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0 - // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0 - const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower; + // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0 + // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0 + // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0 + + // 13-bit Shutter register + const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0); + const uint8_t Shutter_Lower = buf.data.Shutter_Lower; + + const uint16_t shutter = (Shutter_Upper << 8) | Shutter_Lower; - bool data_valid = true; + // Motion since last report and Surface quality non-zero + const bool motion_detected = buf.data.Motion & Motion_Bit::MOT; + + // Number of Features = SQUAL * 4 + bool data_valid = (buf.data.SQUAL > 0); switch (_mode) { case Mode::Bright: + + // quality < 25 (0x19) and shutter >= 8176 (0x1FF0) if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) { // false motion report, discarding perf_count(_false_motion_perf); - ResetAccumulatedData(); data_valid = false; } + // shutter >= 8190 (0x1FFE), raw data sum < 60 (0x3C) if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) { // Bright -> LowLight _bright_to_low_counter++; - if (_bright_to_low_counter >= 10) { // AND valid for 10 consecutive frames + if (_bright_to_low_counter >= 10) { ChangeMode(Mode::LowLight); } @@ -696,28 +718,31 @@ void PAW3902::RunImpl() break; case Mode::LowLight: + + // quality < 70 (0x46) and shutter >= 8176 (0x1FF0) if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) { // false motion report, discarding perf_count(_false_motion_perf); - ResetAccumulatedData(); data_valid = false; } + // shutter >= 8190 (0x1FFE) and raw data sum < 90 (0x5A) if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) { // LowLight -> SuperLowLight _low_to_bright_counter = 0; _low_to_superlow_counter++; - if (_low_to_superlow_counter >= 10) { // AND valid for 10 consecutive frames + if (_low_to_superlow_counter >= 10) { ChangeMode(Mode::SuperLowLight); } } else if (shutter < 0x0BB8) { // LowLight -> Bright + // shutter < 0x0BB8 (3000) _low_to_bright_counter++; _low_to_superlow_counter = 0; - if (_low_to_bright_counter >= 10) { // AND valid for 10 consecutive frames + if (_low_to_bright_counter >= 10) { ChangeMode(Mode::Bright); } @@ -729,18 +754,29 @@ void PAW3902::RunImpl() break; case Mode::SuperLowLight: + + // quality < 85 (0x55) and shutter >= 3008 (0x0BC0) if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) { // false motion report, discarding perf_count(_false_motion_perf); - ResetAccumulatedData(); data_valid = false; } - if (shutter < 0x03E8) { + // shutter < 500 (0x01F4) + if (shutter < 0x01F4) { + // should not operate with Shutter < 0x01F4 in Mode 2 + _superlow_to_low_counter++; + + if (_superlow_to_low_counter >= 10) { + ChangeMode(Mode::LowLight); + } + + } else if (shutter < 0x03E8) { // SuperLowLight -> LowLight + // shutter < 1000 (0x03E8) _superlow_to_low_counter++; - if (_superlow_to_low_counter >= 10) { // AND valid for 10 consecutive frames + if (_superlow_to_low_counter >= 10) { ChangeMode(Mode::LowLight); } @@ -751,94 +787,74 @@ void PAW3902::RunImpl() break; } - if (data_valid && (buf.data.SQUAL > 0)) { - const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); - const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); - - _flow_dt_sum_usec += dt_flow; - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - _flow_sample_counter++; - _flow_quality_sum += buf.data.SQUAL; - - } else { - ResetAccumulatedData(); - return; - } + if (data_valid) { + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < COLLECT_TIME) { - return; - } + report.integration_timespan_us = _scheduled_interval_us; + report.quality = buf.data.SQUAL; - optical_flow_s report{}; - report.timestamp = timestamp_sample; - //report.device_id = get_device_id(); + // set specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity - float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians - float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians + switch (_mode) { + case Mode::Bright: + report.mode = sensor_optical_flow_s::MODE_BRIGHT; + break; - // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow_x_integral = pixel_flow_rotated(0); - report.pixel_flow_y_integral = pixel_flow_rotated(1); + case Mode::LowLight: + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + break; - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + case Mode::SuperLowLight: + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; + break; + } - report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0; + if (motion_detected) { + // only populate flow if data valid (motion and quality > 0) + const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); + const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); - // No gyro on this board - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + // rotate measurements in yaw from sensor frame to body frame + const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; - // set (conservative) specs according to datasheet - report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s - report.min_ground_distance = 0.08f; // Datasheet: 80mm - report.max_ground_distance = 30.0f; // Datasheet: infinity + // datasheet provides 11.914 CPI (count per inch) scaling per meter of height + static constexpr float PIXART_RESOLUTION = 11.914f; // counts per inch (CPI) per meter (from surface) + static constexpr float INCHES_PER_METER = 39.3701f; + // CPI/m -> radians + static constexpr float SCALE = 1.f / (PIXART_RESOLUTION * INCHES_PER_METER); - switch (_mode) { - case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; - break; + report.pixel_flow[0] = pixel_flow_rotated(0) * SCALE; + report.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; + } - case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; - break; + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); - case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; - break; + if (report.quality >= 1) { + _last_good_data = report.timestamp_sample; + } } - report.timestamp = hrt_absolute_time(); - _optical_flow_pub.publish(report); - - ResetAccumulatedData(); -} - -void PAW3902::ResetAccumulatedData() -{ - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; + perf_end(_cycle_perf); } void PAW3902::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); + perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_comms_errors); + perf_print_counter(_reset_perf); perf_print_counter(_false_motion_perf); - perf_print_counter(_register_write_fail_perf); perf_print_counter(_mode_change_bright_perf); perf_print_counter(_mode_change_low_light_perf); perf_print_counter(_mode_change_super_low_light_perf); + perf_print_counter(_no_motion_interrupt_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 87d0d96c27d1..11951be38338 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -34,12 +34,12 @@ /** * @file PAW3902.hpp * - * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI. + * Driver for the PAW3902JF-TXQT: Optical Motion Tracking Chip */ #pragma once -#include "PixArt_PAW3902JF_Registers.hpp" +#include "PixArt_PAW3902_Registers.hpp" #include #include @@ -48,26 +48,22 @@ #include #include #include -#include #include #include -#include +#include using namespace time_literals; -using namespace PixArt_PAW3902JF; +using namespace PixArt_PAW3902; -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define DIR_READ(a) ((a) & 0x7f) +#define DIR_WRITE(a) ((a) | Bit7) +#define DIR_READ(a) ((a) & 0x7F) class PAW3902 : public device::SPI, public I2CSPIDriver { public: - PAW3902(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode, - spi_drdy_gpio_t drdy_gpio, float yaw_rotation_degrees = NAN); + PAW3902(const I2CSPIDriverConfig &config); virtual ~PAW3902(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; @@ -81,55 +77,61 @@ class PAW3902 : public device::SPI, public I2CSPIDriver int probe() override; + void Reset(); + static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); bool DataReadyInterruptConfigure(); bool DataReadyInterruptDisable(); - uint8_t RegisterRead(uint8_t reg, int retries = 3); + uint8_t RegisterRead(uint8_t reg); void RegisterWrite(uint8_t reg, uint8_t data); - bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5); - bool Reset(); + void Configure(); + + bool ChangeMode(Mode newMode, bool force = false); + + void ConfigureModeBright(); + void ConfigureModeLowLight(); + void ConfigureModeSuperLowLight(); void EnableLed(); - void ModeBright(); - void ModeLowLight(); - void ModeSuperLowLight(); + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - bool ChangeMode(Mode newMode, bool force = false); + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; + perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; + perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; + perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; + perf_counter_t _no_motion_interrupt_perf{nullptr}; + + const spi_drdy_gpio_t _drdy_gpio; - void ResetAccumulatedData(); + matrix::Dcmf _rotation; - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + int _discard_reading{3}; - perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; - perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; - perf_counter_t _register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")}; - perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; - perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; - perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; + Mode _mode{Mode::LowLight}; - static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0}; - const spi_drdy_gpio_t _drdy_gpio; + int _bright_to_low_counter{0}; + int _low_to_superlow_counter{0}; + int _low_to_bright_counter{0}; + int _superlow_to_low_counter{0}; - uint64_t _previous_collect_timestamp{0}; - uint64_t _flow_dt_sum_usec{0}; - uint8_t _flow_sample_counter{0}; - uint16_t _flow_quality_sum{0}; + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; - matrix::Dcmf _rotation; + hrt_abstime _last_write_time{0}; + hrt_abstime _last_read_time{0}; - int _flow_sum_x{0}; - int _flow_sum_y{0}; + // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) + static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s; - Mode _mode{Mode::LowLight}; - uint8_t _bright_to_low_counter{0}; - uint8_t _low_to_superlow_counter{0}; - uint8_t _low_to_bright_counter{0}; - uint8_t _superlow_to_low_counter{0}; + hrt_abstime _last_good_data{0}; + hrt_abstime _last_reset{0}; }; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp deleted file mode 100644 index 321295797ae3..000000000000 --- a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp +++ /dev/null @@ -1,103 +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. - * - ****************************************************************************/ - -#pragma once - -namespace PixArt_PAW3902JF -{ - -static constexpr uint8_t PRODUCT_ID = 0x49; -static constexpr uint8_t REVISION_ID = 0x01; -static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6; - -static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps - -static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface - -// Various time delay needed for PAW3902 -static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us -static constexpr uint32_t TIME_us_TSRAD = 2; - -enum Register : uint8_t { - Product_ID = 0x00, - Revision_ID = 0x01, - Motion = 0x02, - Delta_X_L = 0x03, - Delta_X_H = 0x04, - Delta_Y_L = 0x05, - Delta_Y_H = 0x06, - Squal = 0x07, - RawData_Sum = 0x08, - Maximum_RawData = 0x09, - Minimum_RawData = 0x0A, - Shutter_Lower = 0x0B, - Shutter_Upper = 0x0C, - - Observation = 0x15, - Motion_Burst = 0x16, - - Power_Up_Reset = 0x3A, - - Resolution = 0x4E, - - Inverse_Product_ID = 0x5F, -}; - -enum Product_ID_Bit : uint8_t { - Reset = 0x5A, -}; - -enum class Mode { - Bright = 0, - LowLight = 1, - SuperLowLight = 2, -}; - -struct BURST_TRANSFER { - uint8_t Motion; - uint8_t Observation; - uint8_t Delta_X_L; - uint8_t Delta_X_H; - uint8_t Delta_Y_L; - uint8_t Delta_Y_H; - uint8_t SQUAL; - uint8_t RawData_Sum; - uint8_t Maximum_RawData; - uint8_t Minimum_RawData; - uint8_t Shutter_Upper; - uint8_t Shutter_Lower; -}; - -} diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp new file mode 100644 index 000000000000..eceb49020050 --- /dev/null +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace PixArt_PAW3902 +{ + +static constexpr uint8_t PRODUCT_ID = 0x49; +static constexpr uint8_t REVISION_ID = 0x01; +static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6; + +static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps + +static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface + +// Various time delays +static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us) +static constexpr uint32_t TIME_TSWR_us = 6; // SPI Time Between Write and Read Commands +static constexpr uint32_t TIME_TSRW_TSRR_us = 2; // SPI Time Between Read And Subsequent Commands (actually 1.5us) +static constexpr uint32_t TIME_TSRAD_us = 2; // SPI Read Address-Data Delay + +enum Register : uint8_t { + Product_ID = 0x00, + Revision_ID = 0x01, + Motion = 0x02, + Delta_X_L = 0x03, + Delta_X_H = 0x04, + Delta_Y_L = 0x05, + Delta_Y_H = 0x06, + Squal = 0x07, + RawData_Sum = 0x08, + Maximum_RawData = 0x09, + Minimum_RawData = 0x0A, + Shutter_Lower = 0x0B, + Shutter_Upper = 0x0C, + + Observation = 0x15, + Motion_Burst = 0x16, + + Power_Up_Reset = 0x3A, + + Resolution = 0x4E, + + Inverse_Product_ID = 0x5F, +}; + +enum Motion_Bit : uint8_t { + MOT = Bit7, // Motion since last report +}; + +enum class Mode { + Bright = 0, + LowLight = 1, + SuperLowLight = 2, +}; + +struct BURST_TRANSFER { + uint8_t Motion; + uint8_t Observation; + uint8_t Delta_X_L; + uint8_t Delta_X_H; + uint8_t Delta_Y_L; + uint8_t Delta_Y_H; + uint8_t SQUAL; + uint8_t RawData_Sum; + uint8_t Maximum_RawData; + uint8_t Minimum_RawData; + uint8_t Shutter_Upper; + uint8_t Shutter_Lower; +}; + +} diff --git a/src/drivers/optical_flow/paw3902/parameters.c b/src/drivers/optical_flow/paw3902/parameters.c index 96fa1ce316fc..e2ce1da53365 100644 --- a/src/drivers/optical_flow/paw3902/parameters.c +++ b/src/drivers/optical_flow/paw3902/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * PAW3902 & PAW3903 Optical Flow + * PAW3902/PAW3903 Optical Flow * * @reboot_required true * diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index 65d01defa3c5..f8f383f1ee1c 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -43,42 +43,24 @@ void PAW3902::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *PAW3902::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - PAW3902 *instance = new PAW3902(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.bus_frequency, - cli.spi_mode, iterator.DRDYGPIO(), cli.custom1); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - extern "C" __EXPORT int paw3902_main(int argc, char *argv[]) { int ch = 0; using ThisDriver = PAW3902; BusCLIArguments cli{false, true}; + cli.custom1 = -1; cli.spi_mode = SPIDEV_MODE0; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "Y:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) { switch (ch) { case 'Y': - cli.custom1 = atoi(cli.optarg()); + cli.custom1 = atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); @@ -89,13 +71,11 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/optical_flow/pmw3901/Kconfig b/src/drivers/optical_flow/pmw3901/Kconfig new file mode 100644 index 000000000000..f559f13d370e --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OPTICAL_FLOW_PMW3901 + bool "pmw3901" + default n + ---help--- + Enable support for pmw3901 \ No newline at end of file diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index 006408fd3521..7c5506c81dff 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -35,13 +35,12 @@ static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us -PMW3901::PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_FLOW_DEVTYPE_PMW3901, MODULE_NAME, bus, devid, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), +PMW3901::PMW3901(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")), _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")), - _yaw_rotation(yaw_rotation) + _yaw_rotation(config.rotation) { } @@ -208,16 +207,6 @@ PMW3901::sensorInit() int PMW3901::init() { - // get yaw rotation from sensor frame to body frame - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _yaw_rotation = (enum Rotation)val; - } - /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ SPI::set_lockmode(LOCK_THREADS); @@ -327,28 +316,24 @@ PMW3901::RunImpl() delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians - optical_flow_s report{}; - report.timestamp = timestamp; + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp; - report.pixel_flow_x_integral = static_cast(delta_x); - report.pixel_flow_y_integral = static_cast(delta_y); + report.pixel_flow[0] = static_cast(delta_x); + report.pixel_flow[1] = static_cast(delta_y); - // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + // rotate measurements in yaw from sensor frame to body frame float zeroval = 0.0f; - rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); + rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - report.sensor_id = 0; report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0; - /* No gyro on this board */ - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s @@ -361,7 +346,8 @@ PMW3901::RunImpl() _flow_sample_counter = 0; _flow_quality_sum = 0; - _optical_flow_pub.publish(report); + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index 6840f87bf501..2c845fbdcba7 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -45,10 +45,9 @@ #include #include #include -#include #include #include -#include +#include #include /* Configuration Constants */ @@ -65,13 +64,10 @@ class PMW3901 : public device::SPI, public I2CSPIDriver { public: - PMW3901(I2CSPIBusOption bus_option, int bus, int devid, enum Rotation yaw_rotation, int bus_frequency, - spi_mode_e spi_mode); + PMW3901(const I2CSPIDriverConfig &config); virtual ~PMW3901(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); virtual int init(); @@ -87,14 +83,14 @@ class PMW3901 : public device::SPI, public I2CSPIDriver const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz) - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; uint64_t _previous_collect_timestamp{0}; - enum Rotation _yaw_rotation; + enum Rotation _yaw_rotation {}; int _flow_sum_x{0}; int _flow_sum_y{0}; diff --git a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp index bc740950289c..c68f2e4fc8f8 100644 --- a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp +++ b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018, 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 @@ -46,25 +46,6 @@ PMW3901::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *PMW3901::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - PMW3901 *instance = new PMW3901(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - int pmw3901_main(int argc, char *argv[]) { @@ -74,15 +55,15 @@ pmw3901_main(int argc, char *argv[]) cli.spi_mode = SPIDEV_MODE0; cli.default_spi_frequency = PMW3901_SPI_BUS_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); + cli.rotation = (enum Rotation)atoi(cli.optArg()); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/optical_flow/px4flow/Kconfig b/src/drivers/optical_flow/px4flow/Kconfig new file mode 100644 index 000000000000..497860f0c5d7 --- /dev/null +++ b/src/drivers/optical_flow/px4flow/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OPTICAL_FLOW_PX4FLOW + bool "px4flow" + default n + ---help--- + Enable support for px4flow \ No newline at end of file diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index c1f9758edf76..5002a5106de2 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -41,8 +41,6 @@ #include #include -#include -#include #include #include #include @@ -50,8 +48,7 @@ #include #include #include -#include -#include +#include /* Configuration Constants */ #define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 @@ -75,12 +72,9 @@ class PX4FLOW: public device::I2C, public I2CSPIDriver { public: - PX4FLOW(I2CSPIBusOption bus_option, int bus, int address, uint8_t sonar_rotation, int bus_frequency, - int conversion_interval = PX4FLOW_CONVERSION_INTERVAL_DEFAULT, enum Rotation rotation = ROTATION_NONE); + PX4FLOW(const I2CSPIDriverConfig &config); virtual ~PX4FLOW(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; @@ -92,28 +86,19 @@ class PX4FLOW: public device::I2C, public I2CSPIDriver * and start a new one. */ void RunImpl(); -protected: - int probe() override; private: + int probe() override; - uint8_t _sonar_rotation; bool _sensor_ok{false}; bool _collect_phase{false}; - uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; - uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - enum Rotation _sensor_rotation; - float _sensor_min_range{0.0f}; - float _sensor_max_range{0.0f}; - float _sensor_max_flow_rate{0.0f}; - i2c_frame _frame; - i2c_integral_frame _frame_integral; /** * Test whether the device supported by the driver is present at a @@ -137,16 +122,11 @@ class PX4FLOW: public device::I2C, public I2CSPIDriver }; -extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); - -PX4FLOW::PX4FLOW(I2CSPIBusOption bus_option, int bus, int address, uint8_t sonar_rotation, int bus_frequency, - int conversion_interval, enum Rotation rotation) : - I2C(DRV_FLOW_DEVTYPE_PX4FLOW, MODULE_NAME, bus, address, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), - _sonar_rotation(sonar_rotation), +PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _sensor_rotation(rotation) + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { } @@ -170,44 +150,6 @@ PX4FLOW::init() /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward - param_get(rot, &val); - - _sensor_rotation = (enum Rotation)val; - } - - /* get operational limits of the sensor */ - param_t hmin = param_find("SENS_FLOW_MINHGT"); - - if (hmin != PARAM_INVALID) { - float val = 0.7; - param_get(hmin, &val); - - _sensor_min_range = val; - } - - param_t hmax = param_find("SENS_FLOW_MAXHGT"); - - if (hmax != PARAM_INVALID) { - float val = 3.0; - param_get(hmax, &val); - - _sensor_max_range = val; - } - - param_t ratemax = param_find("SENS_FLOW_MAXR"); - - if (ratemax != PARAM_INVALID) { - float val = 2.5; - param_get(ratemax, &val); - - _sensor_max_flow_rate = val; - } - start(); return ret; @@ -273,6 +215,8 @@ PX4FLOW::collect() return ret; } + i2c_integral_frame _frame_integral{}; + if (PX4FLOW_REG == 0) { memcpy(&_frame, val, I2C_FRAME_SIZE); memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); @@ -283,52 +227,37 @@ PX4FLOW::collect() } - optical_flow_s report{}; + DeviceId device_id; + device_id.devid = get_device_id(); + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; + device_id.devid_s.address = get_i2c_address(); + + sensor_optical_flow_s report{}; + + report.timestamp_sample = hrt_absolute_time(); + report.device_id = device_id.devid; + + report.pixel_flow[0] = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians + report.pixel_flow[1] = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians + + report.delta_angle_available = true; + report.delta_angle[0] = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[1] = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[2] = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians + + report.distance_m = static_cast(_frame_integral.ground_distance) / 1000.f; // convert to meters + report.distance_available = true; + + report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds + + report.quality = _frame_integral.qual; // 0:bad ; 255 max quality + + report.max_flow_rate = 2.5f; + report.min_ground_distance = PX4FLOW_MIN_DISTANCE; + report.max_ground_distance = PX4FLOW_MAX_DISTANCE; report.timestamp = hrt_absolute_time(); - report.pixel_flow_x_integral = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians - report.pixel_flow_y_integral = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians - report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout; - report.ground_distance_m = static_cast(_frame_integral.ground_distance) / 1000.0f;//convert to meters - report.quality = _frame_integral.qual; //0:bad ; 255 max quality - report.gyro_x_rate_integral = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians - report.gyro_y_rate_integral = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians - report.gyro_z_rate_integral = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians - report.integration_timespan = _frame_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds - report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius - report.sensor_id = 0; - report.max_flow_rate = _sensor_max_flow_rate; - report.min_ground_distance = _sensor_min_range; - report.max_ground_distance = _sensor_max_range; - - /* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */ - float zeroval = 0.0f; - - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); - - _px4flow_topic.publish(report); - - /* publish to the distance_sensor topic as well */ - if (_distance_sensor_topic.get_instance() == 0) { - distance_sensor_s distance_report{}; - DeviceId device_id; - device_id.devid = get_device_id(); - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; - - distance_report.timestamp = report.timestamp; - distance_report.min_distance = PX4FLOW_MIN_DISTANCE; - distance_report.max_distance = PX4FLOW_MAX_DISTANCE; - distance_report.current_distance = report.ground_distance_m; - distance_report.variance = 0.0f; - distance_report.signal_quality = -1; - distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - distance_report.device_id = device_id.devid; - distance_report.orientation = _sonar_rotation; - - _distance_sensor_topic.publish(distance_report); - } + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); @@ -378,48 +307,17 @@ PX4FLOW::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 35, "Rotation (default=downwards)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *PX4FLOW::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +extern "C" __EXPORT int px4flow_main(int argc, char *argv[]) { - PX4FLOW *instance = new PX4FLOW(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, cli.orientation, - cli.bus_frequency); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - -int -px4flow_main(int argc, char *argv[]) -{ - int ch; using ThisDriver = PX4FLOW; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED; - cli.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.orientation = atoi(cli.optarg()); - break; - } - } - - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); @@ -429,14 +327,20 @@ px4flow_main(int argc, char *argv[]) BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PX4FLOW); if (!strcmp(verb, "start")) { + // px4flow can require more time to fully start and be accessible + static constexpr uint64_t STARTUP_MIN_TIME_US = 6'000'000; + const hrt_abstime time_now_us = hrt_absolute_time(); + + if (time_now_us < STARTUP_MIN_TIME_US) { + px4_usleep(STARTUP_MIN_TIME_US - time_now_us); + } + return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/optical_flow/thoneflow/Kconfig b/src/drivers/optical_flow/thoneflow/Kconfig new file mode 100644 index 000000000000..3a1ab3fa8161 --- /dev/null +++ b/src/drivers/optical_flow/thoneflow/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OPTICAL_FLOW_THONEFLOW + bool "thoneflow" + default n + ---help--- + Enable support for thoneflow \ No newline at end of file diff --git a/src/drivers/optical_flow/thoneflow/thoneflow.cpp b/src/drivers/optical_flow/thoneflow/thoneflow.cpp index 62b6d2160edc..91734a26a333 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -50,10 +50,10 @@ #include #include #include -#include -#include -#include -#include +#include + +#include +#include #include "thoneflow_parser.h" @@ -71,21 +71,20 @@ class Thoneflow : public px4::ScheduledWorkItem void print_info(); private: - char _port[20]; - Rotation _rotation; - int _cycle_interval; - int _fd; - char _linebuf[5]; - unsigned _linebuf_index; - THONEFLOW_PARSE_STATE _parse_state; + char _port[20] {}; + Rotation _rotation{ROTATION_NONE}; + int _cycle_interval{10526}; + int _fd{-1}; + char _linebuf[5] {}; + unsigned _linebuf_index{0}; + THONEFLOW_PARSE_STATE _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC}; - hrt_abstime _last_read; + hrt_abstime _last_read{0}; - optical_flow_s _report; - orb_advert_t _optical_flow_pub; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; /** * Initialise the automatic measurement state machine and start it. @@ -106,23 +105,8 @@ class Thoneflow : public px4::ScheduledWorkItem }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]); - Thoneflow::Thoneflow(const char *port) : - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _rotation(Rotation(0)), - _cycle_interval(10526), - _fd(-1), - _linebuf_index(0), - _parse_state(THONEFLOW_PARSE_STATE0_UNSYNC), - _last_read(0), - _report(), - _optical_flow_pub(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")), - _comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err")) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); @@ -206,41 +190,6 @@ Thoneflow::init() ret = PX4_ERROR; break; } - - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _rotation = Rotation(val); - } - - /* Initialise report structure */ - /* No gyro on this board */ - _report.gyro_x_rate_integral = NAN; - _report.gyro_y_rate_integral = NAN; - _report.gyro_z_rate_integral = NAN; - - /* Conservative specs according to datasheet */ - _report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s - _report.min_ground_distance = 0.1f; // Datasheet: 80mm - _report.max_ground_distance = 30.0f; // Datasheet: infinity - - /* Integrated flow is sent at 66fps */ - _report.frame_count_since_last_readout = 1; - _report.integration_timespan = 10526; // microseconds - - /* Get a publish handle on the optical flow topic */ - _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report); - - if (_optical_flow_pub == nullptr) { - PX4_ERR("Failed to create optical_flow object"); - ret = PX4_ERROR; - break; - } - } while (0); /* Close the fd */ @@ -299,20 +248,33 @@ Thoneflow::collect() _last_read = hrt_absolute_time(); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = hrt_absolute_time(); + /* Parse each byte of read buffer */ for (int i = 0; i < ret; i++) { - valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report); + valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report); } /* Publish most recent valid measurement */ if (valid) { - _report.timestamp = hrt_absolute_time(); + + report.device_id = 0; // TODO get_device_id(); + report.integration_timespan_us = 10526; // microseconds /* Rotate measurements from sensor frame to body frame */ float zeroval = 0.0f; - rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval); + rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report); + // Conservative specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity + + report.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(report); } /* Bytes left to parse */ @@ -466,7 +428,7 @@ Serial bus driver for the ThoneFlow-3901U optical flow sensor. Most boards are configured to enable/start the driver on a specified UART using the SENS_TFLOW_CFG parameter. -Setup/usage information: https://docs.px4.io/en/sensor/thoneflow.html +Setup/usage information: https://docs.px4.io/master/en/sensor/pmw3901.html#thone-thoneflow-3901u ### Examples @@ -478,16 +440,15 @@ Stop driver PRINT_MODULE_USAGE_NAME("thoneflow", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information"); } } // namespace -int -thoneflow_main(int argc, char *argv[]) +extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]) { int ch; const char *device_path = ""; @@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[]) thoneflow::usage(); return -1; } - } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { + } else if (!strcmp(argv[myoptind], "stop")) { return thoneflow::stop(); - } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { thoneflow::info(); return 0; } diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp index 28cc5a56936e..3534af002abb 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -61,7 +61,7 @@ const char *parser_state[] = { #endif bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *flow) + sensor_optical_flow_s *flow) { bool parsed_packet = false; @@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH // Checksum valid, populate sensor report int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0]; int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2]; - flow->pixel_flow_x_integral = static_cast(delta_x) * (3.52e-3f); - flow->pixel_flow_y_integral = static_cast(delta_y) * (3.52e-3f); + flow->pixel_flow[0] = static_cast(delta_x) * (3.52e-3f); + flow->pixel_flow[1] = static_cast(delta_y) * (3.52e-3f); *state = THONEFLOW_PARSE_STATE7_CHECKSUM; } else { diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h index fd27b8c35539..d63906fdc64a 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Data Format for ThoneFlow 3901U // =============================== @@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE { }; bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *report); + sensor_optical_flow_s *report); diff --git a/src/drivers/osd/Kconfig b/src/drivers/osd/Kconfig new file mode 100644 index 000000000000..ebaf4de51ea2 --- /dev/null +++ b/src/drivers/osd/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OSD + bool "osd" + default n + ---help--- + Enable support for osd \ No newline at end of file diff --git a/src/drivers/osd/atxxxx/Kconfig b/src/drivers/osd/atxxxx/Kconfig new file mode 100644 index 000000000000..8fa85e50f5ea --- /dev/null +++ b/src/drivers/osd/atxxxx/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_OSD_ATXXXX + bool "atxxxx" + default n + ---help--- + Enable support for atxxxx \ No newline at end of file diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index 8b40635b9702..b504b943c79a 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -46,10 +46,10 @@ using namespace time_literals; static constexpr uint32_t OSD_UPDATE_RATE{50_ms}; // 20 Hz -OSDatxxxx::OSDatxxxx(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_OSD_DEVTYPE_ATXXXX, MODULE_NAME, bus, devid, spi_mode, bus_frequency), +OSDatxxxx::OSDatxxxx(const I2CSPIDriverConfig &config) : + SPI(config), ModuleParams(nullptr), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus) + I2CSPIDriver(config) { } @@ -84,6 +84,10 @@ OSDatxxxx::init() } } + if (ret == PX4_OK) { + start(); + } + return ret; } @@ -404,11 +408,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state) flight_mode = "AUTO"; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - flight_mode = "FAILURE"; - break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: flight_mode = "ACRO"; break; @@ -503,27 +502,6 @@ It can be enabled with the OSD_ATXXXX_CFG parameter. PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *OSDatxxxx::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - OSDatxxxx *instance = new OSDatxxxx(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), - cli.bus_frequency, cli.spi_mode); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - instance->start(); - - return instance; -} - int atxxxx_main(int argc, char *argv[]) { diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index 27db949d4f18..e821e65deb32 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -68,11 +68,9 @@ extern "C" __EXPORT int atxxxx_main(int argc, char *argv[]); class OSDatxxxx : public device::SPI, public ModuleParams, public I2CSPIDriver { public: - OSDatxxxx(I2CSPIBusOption bus_option, int bus, int devid, int bus_frequency, spi_mode_e spi_mode); + OSDatxxxx(const I2CSPIDriverConfig &config); virtual ~OSDatxxxx() = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); int init() override; diff --git a/src/drivers/pca9685/Kconfig b/src/drivers/pca9685/Kconfig new file mode 100644 index 000000000000..81898b61a8bf --- /dev/null +++ b/src/drivers/pca9685/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PCA9685 + bool "pca9685" + default n + ---help--- + Enable support for pca9685 \ No newline at end of file diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index 84b7604b1dbe..588c624c4476 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -116,11 +116,9 @@ using namespace time_literals; class PCA9685 : public device::I2C, public I2CSPIDriver { public: - PCA9685(I2CSPIBusOption bus_option, int bus, int bus_frequency); + PCA9685(const I2CSPIDriverConfig &config); ~PCA9685() override = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void print_status(); @@ -177,9 +175,9 @@ class PCA9685 : public device::I2C, public I2CSPIDriver }; -PCA9685::PCA9685(I2CSPIBusOption bus_option, int bus, int bus_frequency) : - I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, ADDR, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), +PCA9685::PCA9685(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), _mode(IOX_MODE_ON), _i2cpwm_interval(1_s / 60.0f), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), @@ -436,29 +434,12 @@ PCA9685::print_usage() PRINT_MODULE_USAGE_NAME("pca9685", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x40); PRINT_MODULE_USAGE_COMMAND("reset"); PRINT_MODULE_USAGE_COMMAND_DESCR("test", "enter test mode"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *PCA9685::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - PCA9685 *instance = new PCA9685(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - void PCA9685::custom_method(const BusCLIArguments &cli) { switch (cli.custom1) { @@ -473,6 +454,7 @@ extern "C" int pca9685_main(int argc, char *argv[]) using ThisDriver = PCA9685; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 100000; + cli.i2c_address = ADDR; const char *verb = cli.parseDefaultArguments(argc, argv); diff --git a/src/drivers/pca9685_pwm_out/CMakeLists.txt b/src/drivers/pca9685_pwm_out/CMakeLists.txt index dfb2a4637517..a76fb49bbb17 100644 --- a/src/drivers/pca9685_pwm_out/CMakeLists.txt +++ b/src/drivers/pca9685_pwm_out/CMakeLists.txt @@ -37,8 +37,9 @@ px4_add_module( SRCS main.cpp PCA9685.cpp + MODULE_CONFIG + module.yaml DEPENDS mixer mixer_module - output_limit ) diff --git a/src/drivers/pca9685_pwm_out/Kconfig b/src/drivers/pca9685_pwm_out/Kconfig new file mode 100644 index 000000000000..84cc71cda739 --- /dev/null +++ b/src/drivers/pca9685_pwm_out/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PCA9685_PWM_OUT + bool "pca9685_pwm_out" + default n + ---help--- + Enable support for pca9685_pwm_out \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index a8e95b0f1dd3..c2a1af4b16cf 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -35,6 +35,8 @@ #include #include "PCA9685.h" +#include + using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): @@ -255,4 +257,4 @@ void PCA9685::triggerRestart() PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); return; } -} \ No newline at end of file +} diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index cd4d2a7b28d1..c748d526e7b5 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * 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 @@ -49,6 +49,8 @@ #include "PCA9685.h" +#include + #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -115,7 +117,7 @@ class PCA9685Wrapper : public cdev::CDev, public ModuleBase, pub uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - MixingOutput _mixing_output{PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; + MixingOutput _mixing_output{"PCA9685", PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : @@ -124,8 +126,10 @@ PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _schd_rate_limit(schd_rate_limit) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + if (!_mixing_output.useDynamicMixing()) { + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + } } PCA9685Wrapper::~PCA9685Wrapper() @@ -156,7 +160,7 @@ int PCA9685Wrapper::init() _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); - this->ChangeWorkQeue(px4::device_bus_to_wq(pca9685->get_device_id())); + this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); PX4_INFO("running on I2C bus %d address 0x%.2x", pca9685->get_device_bus(), pca9685->get_device_address()); @@ -173,6 +177,10 @@ void PCA9685Wrapper::updateParams() void PCA9685Wrapper::updatePWMParams() { + if (_mixing_output.useDynamicMixing()) { + return; + } + // update pwm params const char *pname_format_pwm_ch_max[2] = {"PWM_MAIN_MAX%d", "PWM_AUX_MAX%d"}; const char *pname_format_pwm_ch_min[2] = {"PWM_MAIN_MIN%d", "PWM_AUX_MIN%d"}; @@ -369,7 +377,6 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned void PCA9685Wrapper::Run() { if (should_exit()) { - PX4_INFO("PCA9685 stopping."); ScheduleClear(); _mixing_output.unregister(); unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); @@ -382,6 +389,8 @@ void PCA9685Wrapper::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); switch (_state) { @@ -457,24 +466,22 @@ void PCA9685Wrapper::Run() perf_end(_cycle_perf); } -// TODO int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) { - int ret = OK; + SmartLock lock_guard(_lock); - lock(); + int ret = OK; switch (cmd) { case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } @@ -485,9 +492,7 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) break; case PWM_SERVO_SET_ARM_OK: - case PWM_SERVO_SET_FORCE_SAFETY_OFF: case PWM_SERVO_CLEAR_ARM_OK: - case PWM_SERVO_SET_FORCE_SAFETY_ON: case PWM_SERVO_ARM: case PWM_SERVO_DISARM: break; @@ -497,8 +502,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg) break; } - unlock(); - if (ret == -ENOTTY) { ret = CDev::ioctl(filep, cmd, arg); } diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml new file mode 100644 index 000000000000..8833091e3cc9 --- /dev/null +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -0,0 +1,11 @@ +module_name: PCA9685 Output +actuator_output: + output_groups: + - param_prefix: PCA9685 + channel_label: 'Channel' + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } + num_channels: 16 diff --git a/src/drivers/power_monitor/Kconfig b/src/drivers/power_monitor/Kconfig new file mode 100644 index 000000000000..ef809f80d095 --- /dev/null +++ b/src/drivers/power_monitor/Kconfig @@ -0,0 +1,3 @@ +menu "Power monitor" +rsource "*/Kconfig" +endmenu diff --git a/src/drivers/power_monitor/ina226/Kconfig b/src/drivers/power_monitor/ina226/Kconfig new file mode 100644 index 000000000000..05f77beb783d --- /dev/null +++ b/src/drivers/power_monitor/ina226/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_INA226 + bool "ina226" + default n + ---help--- + Enable support for ina226 \ No newline at end of file diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index ee74c612df8f..2c117caf1dce 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -41,15 +41,15 @@ #include "ina226.h" -INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index) : - I2C(DRV_POWER_DEVTYPE_INA226, MODULE_NAME, bus, address, bus_frequency), +INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : + I2C(config), ModuleParams(nullptr), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), + I2CSPIDriver(config), _sample_perf(perf_alloc(PC_ELAPSED, "ina226_read")), _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")), - _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US) + _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; @@ -83,15 +83,10 @@ INA226::INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int _power_lsb = 25 * _current_lsb; // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0, - false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 - ); + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } INA226::~INA226() @@ -107,14 +102,19 @@ int INA226::read(uint8_t address, int16_t &data) { // read desired little-endian value via I2C uint16_t received_bytes; - const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + int ret = PX4_ERROR; - if (ret == PX4_OK) { - data = swap16(received_bytes); + for (size_t i = 0; i < 3; i++) { + ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); - } else { - perf_count(_comms_errors); - PX4_DEBUG("i2c::transfer returned %d", ret); + if (ret == PX4_OK) { + data = swap16(received_bytes); + break; + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } } return ret; @@ -138,7 +138,7 @@ INA226::init() write(INA226_REG_CONFIGURATION, INA226_RST); - _cal = INA226_CONST / (_current_lsb * INA226_SHUNT); + _cal = INA226_CONST / (_current_lsb * _rshunt); if (write(INA226_REG_CALIBRATION, _cal) < 0) { return -3; @@ -231,17 +231,10 @@ INA226::collect() _bus_voltage = _power = _current = _shunt = 0; } - _actuators_sub.copy(&_actuator_controls); - - _battery.updateBatteryStatus( - hrt_absolute_time(), - (float) _bus_voltage * INA226_VSCALE, - (float) _current * _current_lsb, - success, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] - ); + _battery.setConnected(success); + _battery.updateVoltage(static_cast(_bus_voltage * INA226_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); perf_end(_sample_perf); @@ -304,15 +297,10 @@ INA226::RunImpl() ScheduleDelayed(INA226_CONVERSION_INTERVAL); } else { - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0f, - 0.0f, - false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0f - ); + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); if (init() != PX4_OK) { ScheduleDelayed(INA226_INIT_RETRY_INTERVAL_US); diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index 58a377d9085a..a402a079ab7b 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -45,9 +45,7 @@ #include #include #include -#include #include -#include #include #include @@ -152,11 +150,10 @@ using namespace time_literals; class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - INA226(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address, int battery_index); + INA226(const I2CSPIDriverConfig &config, int battery_index); virtual ~INA226(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); void RunImpl(); @@ -203,10 +200,7 @@ class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver #include #include "ina226.h" -I2CSPIDriverBase *INA226::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *INA226::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) { - INA226 *instance = new INA226(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.i2c_address, - cli.custom2); + INA226 *instance = new INA226(config, config.custom1); if (instance == nullptr) { PX4_ERR("alloc failed"); return nullptr; } - if (cli.keep_running) { + if (config.keep_running) { if (instance->force_init() != PX4_OK) { - PX4_INFO("Failed to init INA226 on bus %d, but will try again periodically.", iterator.bus()); + PX4_INFO("Failed to init INA226 on bus %d, but will try again periodically.", config.bus); } } else if (instance->init() != PX4_OK) { @@ -66,17 +95,17 @@ ina226_main(int argc, char *argv[]) cli.i2c_address = INA226_BASEADDR; cli.default_i2c_frequency = 100000; cli.support_keep_running = true; - cli.custom2 = 1; + cli.custom1 = 1; - while ((ch = cli.getopt(argc, argv, "t:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { switch (ch) { case 't': // battery index - cli.custom2 = (int)strtol(cli.optarg(), NULL, 0); + cli.custom1 = (int)strtol(cli.optArg(), NULL, 0); break; } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); return -1; diff --git a/src/drivers/power_monitor/ina226/ina226_params.c b/src/drivers/power_monitor/ina226/ina226_params.c index 8049a3f94920..14038e66e669 100644 --- a/src/drivers/power_monitor/ina226/ina226_params.c +++ b/src/drivers/power_monitor/ina226/ina226_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019, 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 @@ -31,6 +31,16 @@ * ****************************************************************************/ +/** + * Enable INA226 Power Monitor + * + * For systems a INA226 Power Monitor, this should be set to true + * + * @group Sensors + * @boolean + * @reboot_required true +*/ +PARAM_DEFINE_INT32(SENS_EN_INA226, 0); /** * INA226 Power Monitor Config diff --git a/src/drivers/power_monitor/ina228/CMakeLists.txt b/src/drivers/power_monitor/ina228/CMakeLists.txt new file mode 100644 index 000000000000..32af49b463cd --- /dev/null +++ b/src/drivers/power_monitor/ina228/CMakeLists.txt @@ -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. +# +############################################################################ +px4_add_module( + MODULE drivers__ina228 + MAIN ina228 + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ina228_main.cpp + ina228.cpp + DEPENDS + battery + px4_work_queue + ) diff --git a/src/drivers/power_monitor/ina228/Kconfig b/src/drivers/power_monitor/ina228/Kconfig new file mode 100644 index 000000000000..cf5596a1ef84 --- /dev/null +++ b/src/drivers/power_monitor/ina228/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_INA228 + bool "ina228" + default n + ---help--- + Enable support for ina228 diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp new file mode 100644 index 000000000000..581dcbdd7026 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * 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 ina228.cpp + * @author David Sidrane + * + * Driver for the I2C attached INA228 + */ + +#include "ina228.h" + + +INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : + I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config), + _sample_perf(perf_alloc(PC_ELAPSED, "ina228_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")), + _collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")), + _measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")), + _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) +{ + float fvalue = MAX_CURRENT; + _max_current = fvalue; + param_t ph = param_find("INA228_CURRENT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _max_current = fvalue; + } + + _range = _max_current > (MAX_CURRENT - 1.0f) ? INA228_ADCRANGE_HIGH : INA228_ADCRANGE_LOW; + + fvalue = INA228_SHUNT; + _rshunt = fvalue; + ph = param_find("INA228_SHUNT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _rshunt = fvalue; + } + + ph = param_find("INA228_CONFIG"); + int32_t value = INA228_ADCCONFIG; + _config = (uint16_t)value; + + if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) { + _config = (uint16_t)value; + } + + _mode_triggered = ((_config & INA228_MODE_MASK) >> INA228_MODE_SHIFTS) <= + ((INA228_MODE_TEMP_SHUNT_BUS_TRIG & INA228_MODE_MASK) >> + INA228_MODE_SHIFTS); + + _current_lsb = _max_current / DN_MAX; + _power_lsb = 3.2f * _current_lsb; + + // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); +} + +INA228::~INA228() +{ + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_collection_errors); + perf_free(_measure_errors); +} + +int INA228::read(uint8_t address, int16_t &data) +{ + // read desired little-endian value via I2C + int16_t received_bytes; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + + if (ret == PX4_OK) { + data = swap16(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, int32_t &data) +{ + // read desired 24 bit value via I2C + int32_t received_bytes{0}; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 1); + + if (ret == PX4_OK) { + data = swap32(received_bytes) >> ((32 - 24) + 4); // Convert to 20bit value + + // Handle negative 20bit twos complement + if (data & 0x80000) { + data = -((0x000FFFFF & ~data) + 1); + } + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, int64_t &data) +{ + // read desired 40 bit little-endian value via I2C + int64_t received_bytes{0}; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes) - 3); + + if (ret == PX4_OK) { + data = swap64(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::read(uint8_t address, uint16_t &data) +{ + // read desired little-endian value via I2C + uint16_t received_bytes; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + + if (ret == PX4_OK) { + data = swap16(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA228::write(uint8_t address, uint16_t value) +{ + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA228::write(uint8_t address, int16_t value) +{ + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA228::write(uint8_t address, int32_t value) +{ + uint8_t data[4] = {address, ((uint8_t)((value & 0xff0000) >> 16)), ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA228::write(uint8_t address, int64_t value) +{ + uint8_t data[6] = {address, ((uint8_t)((value & 0xff000000) >> 32)), ((uint8_t)((value & 0xff0000) >> 24)), ((uint8_t)((value & 0xff00) >> 16)), ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int +INA228::init() +{ + int ret = PX4_ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != PX4_OK) { + return ret; + } + + write(INA228_REG_CONFIG, (uint16_t)(INA228_RST_RESET | _range)); + + _cal = INA228_CONST * _current_lsb * _rshunt; + + if (_range == INA228_ADCRANGE_LOW) { + _cal *= 4; + } + + if (write(INA228_REG_SHUNTCAL, _cal) < 0) { + return -3; + } + + // Set the CONFIG for max I + + write(INA228_REG_CONFIG, (uint16_t) _range); + + // If we run in continuous mode then start it here + + + if (!_mode_triggered) { + + ret = write(INA228_REG_ADCCONFIG, _config); + + } else { + ret = PX4_OK; + } + + start(); + _sensor_ok = true; + + _initialized = ret == PX4_OK; + return ret; +} + +int +INA228::force_init() +{ + int ret = init(); + + start(); + + return ret; +} + +int +INA228::probe() +{ + uint16_t value{0}; + + if (read(INA228_MANUFACTURER_ID, value) != PX4_OK || value != INA228_MFG_ID_TI) { + PX4_DEBUG("probe mfgid %d", value); + return -1; + } + + if (read(INA228_DEVICE_ID, value) != PX4_OK || INA228_DEVICEID(value) != INA228_MFG_DIE) { + PX4_DEBUG("probe die id %d", value); + return -1; + } + + return PX4_OK; +} + +int +INA228::measure() +{ + int ret = PX4_OK; + + if (_mode_triggered) { + ret = write(INA228_REG_ADCCONFIG, _config); + + if (ret < 0) { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + } + + return ret; +} + +int +INA228::collect() +{ + perf_begin(_sample_perf); + + if (_parameter_update_sub.updated()) { + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + + updateParams(); + } + + // read from the sensor + // Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid. + bool success{true}; + success = success && (read(INA228_REG_VSBUS, _bus_voltage) == PX4_OK); + // success = success && (read(INA228_REG_POWER, _power) == PX4_OK); + success = success && (read(INA228_REG_CURRENT, _current) == PX4_OK); + //success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK); + + if (!success) { + PX4_DEBUG("error reading from sensor"); + _bus_voltage = _power = _current = _shunt = 0; + } + + _battery.setConnected(success); + _battery.updateVoltage(static_cast(_bus_voltage * INA228_VSCALE)); + _battery.updateCurrent(static_cast(_current * _current_lsb)); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + perf_end(_sample_perf); + + if (success) { + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void +INA228::start() +{ + ScheduleClear(); + + /* reset the report ring and state machine */ + _collect_phase = false; + + _measure_interval = INA228_CONVERSION_INTERVAL; + + /* schedule a cycle to start things */ + ScheduleDelayed(5); +} + +void +INA228::RunImpl() +{ + if (_initialized) { + if (_collect_phase) { + /* perform collection */ + if (collect() != PX4_OK) { + perf_count(_collection_errors); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = !_mode_triggered; + + if (_measure_interval > INA228_CONVERSION_INTERVAL) { + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - INA228_CONVERSION_INTERVAL); + return; + } + } + + /* Measurement phase */ + + /* Perform measurement */ + if (measure() != PX4_OK) { + perf_count(_measure_errors); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(INA228_CONVERSION_INTERVAL); + + } else { + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + if (init() != PX4_OK) { + ScheduleDelayed(INA228_INIT_RETRY_INTERVAL_US); + } + } +} + +void +INA228::print_status() +{ + I2CSPIDriverBase::print_status(); + + if (_initialized) { + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + printf("poll interval: %u \n", _measure_interval); + + } else { + PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", + INA228_INIT_RETRY_INTERVAL_US / 1000); + } +} diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h new file mode 100644 index 000000000000..d010d5e4b950 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -0,0 +1,379 @@ +/**************************************************************************** + * + * 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 ina228.h + * + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +/* Configuration Constants */ +#define INA228_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ +// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to +// connect to the INA228 every this many microseconds +#define INA228_INIT_RETRY_INTERVAL_US 500000 + +/* INA228 Registers addresses */ +#define INA228_REG_CONFIG (0x00) +#define INA228_REG_ADCCONFIG (0x01) +#define INA228_REG_SHUNTCAL (0x02) +#define INA228_REG_SHUNTTEMPCO (0x03) +#define INA228_REG_VSHUNT (0x04) +#define INA228_REG_VSBUS (0x05) +#define INA228_REG_DIETEMP (0x06) +#define INA228_REG_CURRENT (0x07) +#define INA228_REG_POWER (0x08) +#define INA228_REG_ENERGY (0x09) +#define INA228_REG_CHARGE (0x0a) +#define INA228_REG_DIAG_ALRT (0x0b) +#define INA228_REG_SOVL (0x0c) +#define INA228_REG_SUVL (0x0d) +#define INA228_REG_BOVL (0x0e) +#define INA228_REG_BUVL (0x0f) +#define INA228_REG_TEMP_LIMIT (0x10) +#define INA228_REG_TPWR_LIMIT (0x11) +#define INA228_MANUFACTURER_ID (0x3e) +#define INA228_DEVICE_ID (0x3f) + +#define INA228_MFG_ID_TI (0x5449) // TI +#define INA228_MFG_DIE (0x228) // INA228 + +/* INA228 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */ +#define INA228_ADCRANGE_SHIFTS (4) +#define INA228_ADCRANGE_MASK (1 << INA228_ADCRANGE_SHIFTS) +# define INA228_ADCRANGE_LOW (1 << INA228_ADCRANGE_SHIFTS) // ± 40.96 mV +# define INA228_ADCRANGE_HIGH (0 << INA228_ADCRANGE_SHIFTS) // ±163.84 mV +#define INA228_TEMPCOMP_SHIFTS (5) +#define INA228_TEMPCOMP_MASK (1 << INA228_TEMPCOMP_SHIFTS) +# define INA228_TEMPCOMP_ENABLE (1 << INA228_TEMPCOMP_SHIFTS) +# define INA228_TEMPCOMP_DISABLE (0 << INA228_TEMPCOMP_SHIFTS) + +#define INA228_CONVDLY_SHIFTS (6) +#define INA228_CONVDLY_MASK (0xff << INA228_CONVDLY_SHIFTS) +# define INA228_CONVDLY2MS(n) ((n) << INA228_CONVDLY_SHIFTS) + +#define INA228_RSTACC_SHIFTS (14) +#define INA228_RSTACC_MASK (1 << INA228_RSTACC_SHIFTS) +# define INA228_RSTACC_CLEAR (1 << INA228_RSTACC_SHIFTS) +# define INA228_RSTACC_NORMAL (0 << INA228_RSTACC_SHIFTS) + +#define INA228_RST_SHIFTS (15) +#define INA228_RST_MASK (1 << INA228_RST_SHIFTS) +# define INA228_RST_RESET (1 << INA228_RST_SHIFTS) +# define INA228_RST_NORMAL (0 << INA228_RST_SHIFTS) + +/* INA228 ADC Configuration (ADC_CONFIG) 16-bit Register (Address = 1h) [reset = FB68h] */ + +#define INA228_MODE_SHIFTS (12) +#define INA228_MODE_MASK (0xf << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUTDOWN_TRIG (0 << INA228_MODE_SHIFTS) +#define INA228_MODE_BUS_TRIG (1 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_TRIG (2 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_BUS_TRIG (3 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_TRIG (4 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_BUS_TRIG (5 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_TRIG (6 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_BUS_TRIG (7 << INA228_MODE_SHIFTS) + +#define INA228_MODE_SHUTDOWN_CONT (8 << INA228_MODE_SHIFTS) +#define INA228_MODE_BUS_CONT (9 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_CONT (10 << INA228_MODE_SHIFTS) +#define INA228_MODE_SHUNT_BUS_CONT (11 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_CONT (12 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_BUS_CONT (13 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_CONT (14 << INA228_MODE_SHIFTS) +#define INA228_MODE_TEMP_SHUNT_BUS_CONT (15 << INA228_MODE_SHIFTS) + +#define INA228_VBUSCT_SHIFTS (9) +#define INA228_VBUSCT_MASK (7 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_50US (0 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_84US (1 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_150US (2 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_280US (3 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_540US (4 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_1052US (5 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_2074US (6 << INA228_VBUSCT_SHIFTS) +#define INA228_VBUSCT_4170US (7 << INA228_VBUSCT_SHIFTS) + +#define INA228_VSHCT_SHIFTS (6) +#define INA228_VSHCT_MASK (7 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_50US (0 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_84US (1 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_150US (2 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_280US (3 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_540US (4 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_1052US (5 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_2074US (6 << INA228_VSHCT_SHIFTS) +#define INA228_VSHCT_4170US (7 << INA228_VSHCT_SHIFTS) + +#define INA228_VTCT_SHIFTS (3) +#define INA228_VTCT_MASK (7 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_50US (0 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_84US (1 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_150US (2 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_280US (3 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_540US (4 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_1052US (5 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_2074US (6 << INA228_VTCT_SHIFTS) +#define INA228_VTCT_4170US (7 << INA228_VTCT_SHIFTS) + +#define INA228_AVERAGES_SHIFTS (0) +#define INA228_AVERAGES_MASK (7 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_1 (0 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_4 (1 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_16 (2 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_64 (3 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_128 (4 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_256 (5 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_512 (6 << INA228_AVERAGES_SHIFTS) +#define INA228_AVERAGES_1024 (7 << INA228_AVERAGES_SHIFTS) + +#define INA228_ADCCONFIG (INA228_MODE_TEMP_SHUNT_BUS_CONT | INA228_VBUSCT_540US | INA228_VSHCT_540US | INA228_VTCT_540US |INA228_AVERAGES_64) + +/* INA228 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */ + +#define INA228_CURRLSB_SHIFTS (0) +#define INA228_CURRLSB_MASK (0x7fff << INA228_CURRLSB_SHIFTS) + +/* INA228 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */ + +#define INA228_TEMPCO_SHIFTS (0) +#define INA228_TEMPCO_MASK (0x1fff << INA228_TEMPCO_SHIFTS) + +/* INA228 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */ + +#define INA228_VSHUNT_SHIFTS (4) +#define INA228_VSHUNT_MASK (UINT32_C(0xffffff) << INA228_VSHUNT_SHIFTS) + +/* INA228 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */ + +#define INA228_VBUS_SHIFTS (4) +#define INA228_VBUS_MASK (UINT32_C(0xffffff) << INA228_VBUS_SHIFTS) + +/* INA228 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */ + +#define INA228_DIETEMP_SHIFTS (0) +#define INA228_DIETEMP_MASK (0xffff << INA228_DIETEMP_SHIFTS) + +/* INA228 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */ + +#define INA228_CURRENT_SHIFTS (4) +#define INA228_CURRENT_MASK (UINT32_C(0xffffff) << INA228_CURRENT_SHIFTS) + +/* INA228 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */ + +#define INA228_POWER_SHIFTS (0) +#define INA228_POWER_MASK (UINT32_C(0xffffff) << INA228_POWER_SHIFTS) + +/* INA228 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */ + +#define INA228_ENERGY_SHIFTS (0) +#define INA228_ENERGY_MASK (UINT64_C(0xffffffffff) << INA228_ENERGY_SHIFTS) + +/* INA228 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ + +#define INA228_CHARGE_SHIFTS (0) +#define INA228_CHARGE_MASK (UINT64_C(0xffffffffff) << INA228_CHARGE_SHIFTS) + + +/* INA228 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */ + +#define INA228_MEMSTAT (1 << 0) // This bit is set to 0 if a checksum error is detected in the device trim memory space +#define INA228_CNVRF (1 << 1) // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion. +#define INA228_POL (1 << 2) // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register. +#define INA228_BUSUL (1 << 3) // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register. +#define INA228_BUSOL (1 << 4) // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register. +#define INA228_SHNTUL (1 << 5) // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register +#define INA228_SHNTOL (1 << 6) // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register. +#define INA228_TMPOL (1 << 7) // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register. +#define INA228_MATHOF (1 << 9) // This bit is set to 1 if an arithmetic operation resulted in an overflow error. +#define INA228_CHARGEOF (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1. +#define INA228_ENERGYOF (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1. +#define INA228_APOL (1 << 12) // Alert Polarity bit sets the Alert pin polarity. +#define INA228_SLOWALER (1 << 13) // ALERT function is asserted on the completed averaged value. This gives the flexibility to delay the ALERT after the averaged value. +#define INA228_CNVR (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed +#define INA228_ALATCH (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been +// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until +// the DIAG_ALRT Register has been read. + +/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */ + +#define INA228_SOVL_SHIFTS (0) +#define INA228_SOVL_MASK (0xffff << INA228_SOVL_SHIFTS) + +/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */ + +#define INA228_SUVL_SHIFTS (0) +#define INA228_SUVL_MASK (0xffff << INA228_SUVL_SHIFTS) + +/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */ + +#define INA228_BOVL_SHIFTS (0) +#define INA228_BOVL_MASK (0xffff << INA228_BOVL_SHIFTS) + +/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */ + +#define INA228_BUVL_SHIFTS (0) +#define INA228_BUVL_MASK (0xffff << INA228_BUVL_SHIFTS) + +/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */ + +#define INA228_TEMP_LIMIT_SHIFTS (0) +#define INA228_TEMP_LIMIT_MASK (0xffff << INA228_TEMP_LIMIT_SHIFTS) + +/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */ + +#define INA228_POWER_LIMIT_SHIFTS (0) +#define INA228_POWER_LIMIT_MASK (0xffff << INA228_POWER_LIMIT_SHIFTS) + +/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */ + +/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2280h] */ + +#define INA228_DEVICE_REV_ID_SHIFTS (0) +#define INA228_DEVICE_REV_ID_MASK (0xf << INA228_DEVICE_REV_ID_SHIFTS) +#define INA228_DEVICEREV_ID(v) (((v) & INA228_DEVICE_REV_ID_MASK) >> INA228_DEVICE_REV_ID_SHIFTS) +#define INA228_DEVICE_ID_SHIFTS (4) +#define INA228_DEVICE_ID_MASK (0xfff << INA228_DEVICE_ID_SHIFTS) +#define INA228_DEVICEID(v) (((v) & INA228_DEVICE_ID_MASK) >> INA228_DEVICE_ID_SHIFTS) + + + +#define INA228_SAMPLE_FREQUENCY_HZ 10 +#define INA228_SAMPLE_INTERVAL_US (1_s / INA228_SAMPLE_FREQUENCY_HZ) +#define INA228_CONVERSION_INTERVAL (INA228_SAMPLE_INTERVAL_US - 7) +#define MAX_CURRENT 327.68f /* Amps */ +#define DN_MAX 524288.0f /* 2^19 */ +#define INA228_CONST 13107.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */ +#define INA228_SHUNT 0.0005f /* Shunt is 500 uOhm */ +#define INA228_VSCALE 1.95e-04f /* LSB of voltage is 195.3125 uV/LSB */ + +#define swap16(w) __builtin_bswap16((w)) +#define swap32(d) __builtin_bswap32((d)) +#define swap64(q) __builtin_bswap64((q)) + +class INA228 : public device::I2C, public ModuleParams, public I2CSPIDriver +{ +public: + INA228(const I2CSPIDriverConfig &config, int battery_index); + virtual ~INA228(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + + /** + * Tries to call the init() function. If it fails, then it will schedule to retry again in + * INA228_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds. + * + * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. + */ + int force_init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_status() override; + +protected: + int probe() override; + +private: + bool _sensor_ok{false}; + unsigned _measure_interval{0}; + bool _collect_phase{false}; + bool _initialized{false}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _collection_errors; + perf_counter_t _measure_errors; + + int32_t _bus_voltage{0}; + int64_t _power{0}; + int32_t _current{0}; + int32_t _shunt{0}; + int16_t _cal{0}; + int16_t _range{INA228_ADCRANGE_HIGH}; + bool _mode_triggered{false}; + + float _max_current{MAX_CURRENT}; + float _rshunt{INA228_SHUNT}; + uint16_t _config{INA228_ADCCONFIG}; + float _current_lsb{_max_current / DN_MAX}; + float _power_lsb{25.0f * _current_lsb}; + + Battery _battery; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + int read(uint8_t address, int16_t &data); + int write(uint8_t address, int16_t data); + + int read(uint8_t address, uint16_t &data); + int write(uint8_t address, uint16_t data); + + int read(uint8_t address, int32_t &data); + int write(uint8_t address, int32_t data); + + int read(uint8_t address, int64_t &data); + int write(uint8_t address, int64_t data); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + int measure(); + int collect(); + +}; diff --git a/src/drivers/power_monitor/ina228/ina228_main.cpp b/src/drivers/power_monitor/ina228/ina228_main.cpp new file mode 100644 index 000000000000..1d1f02e5e7e9 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228_main.cpp @@ -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. + * + ****************************************************************************/ +#include +#include + +#include "ina228.h" + +I2CSPIDriverBase *INA228::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + INA228 *instance = new INA228(config, config.custom1); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (config.keep_running) { + if (instance->force_init() != PX4_OK) { + PX4_INFO("Failed to init INA228 on bus %d, but will try again periodically.", config.bus); + } + + } else if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + return instance; +} + +void +INA228::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for the INA228 power monitor. + +Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address. + +For example, one instance can run on Bus 2, address 0x45, and one can run on Bus 2, address 0x45. + +If the INA228 module is not powered, then by default, initialization of the driver will fail. To change this, use +the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again +every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without +this flag set, the battery must be plugged in before starting the driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ina228", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int +ina228_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = INA228; + BusCLIArguments cli{true, false}; + cli.i2c_address = INA228_BASEADDR; + cli.default_i2c_frequency = 100000; + cli.support_keep_running = true; + cli.custom1 = 1; + + while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { + switch (ch) { + case 't': // battery index + cli.custom1 = (int)strtol(cli.optArg(), NULL, 0); + break; + } + } + + const char *verb = cli.optArg(); + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA228); + + 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/power_monitor/ina228/ina228_params.c b/src/drivers/power_monitor/ina228/ina228_params.c new file mode 100644 index 000000000000..08d55f045617 --- /dev/null +++ b/src/drivers/power_monitor/ina228/ina228_params.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Enable INA228 Power Monitor + * + * For systems a INA228 Power Monitor, this should be set to true + * + * @group Sensors + * @boolean + * @reboot_required true +*/ +PARAM_DEFINE_INT32(SENS_EN_INA228, 0); + +/** + * INA228 Power Monitor Config + * + * @group Sensors + * @min 0 + * @max 65535 + * @decimal 1 + * @increment 1 +*/ +PARAM_DEFINE_INT32(INA228_CONFIG, 63779); + +/** + * INA228 Power Monitor Max Current + * + * @group Sensors + * @min 0.1 + * @max 327.68 + * @decimal 2 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(INA228_CURRENT, 327.68f); + +/** + * INA228 Power Monitor Shunt + * + * @group Sensors + * @min 0.000000001 + * @max 0.1 + * @decimal 10 + * @increment .000000001 + */ +PARAM_DEFINE_FLOAT(INA228_SHUNT, 0.0005f); diff --git a/src/drivers/power_monitor/ina238/CMakeLists.txt b/src/drivers/power_monitor/ina238/CMakeLists.txt new file mode 100644 index 000000000000..311fdad662f1 --- /dev/null +++ b/src/drivers/power_monitor/ina238/CMakeLists.txt @@ -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. +# +############################################################################ +px4_add_module( + MODULE drivers__ina238 + MAIN ina238 + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + ina238_main.cpp + ina238.cpp + DEPENDS + battery + px4_work_queue + ) diff --git a/src/drivers/power_monitor/ina238/Kconfig b/src/drivers/power_monitor/ina238/Kconfig new file mode 100644 index 000000000000..01dde3ec9531 --- /dev/null +++ b/src/drivers/power_monitor/ina238/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_INA238 + bool "ina238" + default n + ---help--- + Enable support for ina238 diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp new file mode 100644 index 000000000000..d8afc1bd636b --- /dev/null +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Driver for the I2C attached INA238 + */ + +#include "ina238.h" + + +INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : + I2C(config), + ModuleParams(nullptr), + I2CSPIDriver(config), + _sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")), + _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), + _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), + _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) +{ + float fvalue = DEFAULT_MAX_CURRENT; + _max_current = fvalue; + param_t ph = param_find("INA238_CURRENT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _max_current = fvalue; + } + + _range = _max_current > (DEFAULT_MAX_CURRENT - 1.0f) ? INA238_ADCRANGE_HIGH : INA238_ADCRANGE_LOW; + + fvalue = DEFAULT_SHUNT; + _rshunt = fvalue; + ph = param_find("INA238_SHUNT"); + + if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { + _rshunt = fvalue; + } + + _current_lsb = _max_current / INA238_DN_MAX; + + // We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0 + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); +} + +INA238::~INA238() +{ + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_collection_errors); +} + +int INA238::read(uint8_t address, uint16_t &data) +{ + // read desired little-endian value via I2C + uint16_t received_bytes; + const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes)); + + if (ret == PX4_OK) { + data = swap16(received_bytes); + + } else { + perf_count(_comms_errors); + PX4_DEBUG("i2c::transfer returned %d", ret); + } + + return ret; +} + +int INA238::write(uint8_t address, uint16_t value) +{ + uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int INA238::init() +{ + int ret = PX4_ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != PX4_OK) { + return ret; + } + + if (write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range)) != PX4_OK) { + return ret; + } + + uint16_t shunt_calibration = static_cast(INA238_CONST * _current_lsb * _rshunt); + + if (_range == INA238_ADCRANGE_LOW) { + shunt_calibration *= 4; + } + + if (write(INA238_REG_SHUNTCAL, shunt_calibration) < 0) { + return -3; + } + + // Set the CONFIG for max I + if (write(INA238_REG_CONFIG, (uint16_t) _range) != PX4_OK) { + return ret; + } + + // Start ADC continous mode here + ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG); + + start(); + _sensor_ok = true; + + _initialized = ret == PX4_OK; + return ret; +} + +int INA238::force_init() +{ + int ret = init(); + + start(); + + return ret; +} + +int INA238::probe() +{ + uint16_t value{0}; + + if (read(INA238_MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) { + PX4_DEBUG("probe mfgid %d", value); + return -1; + } + + if (read(INA238_DEVICE_ID, value) != PX4_OK || ( + INA238_DEVICEID(value) != INA238_MFG_DIE + )) { + PX4_DEBUG("probe die id %d", value); + return -1; + } + + return PX4_OK; +} + + +int INA238::collect() +{ + perf_begin(_sample_perf); + + if (_parameter_update_sub.updated()) { + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + + updateParams(); + } + + // read from the sensor + // Note: If the power module is connected backwards, then the values of _current will be negative but otherwise valid. + bool success{true}; + int16_t bus_voltage{0}; + int16_t current{0}; + + success = success && (read(INA238_REG_VSBUS, bus_voltage) == PX4_OK); + success = success && (read(INA238_REG_CURRENT, current) == PX4_OK); + + 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.updateAndPublishBatteryStatus(hrt_absolute_time()); + + perf_end(_sample_perf); + + if (success) { + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void INA238::start() +{ + ScheduleClear(); + + /* reset the report ring and state machine */ + _collect_phase = false; + + _measure_interval = INA238_CONVERSION_INTERVAL; + + /* schedule a cycle to start things */ + ScheduleDelayed(5); +} + +void INA238::RunImpl() +{ + if (_initialized) { + if (_collect_phase) { + /* perform collection */ + if (collect() != PX4_OK) { + perf_count(_collection_errors); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = true; + + if (_measure_interval > INA238_CONVERSION_INTERVAL) { + /* schedule a fresh cycle call when we are ready to measure again */ + ScheduleDelayed(_measure_interval - INA238_CONVERSION_INTERVAL); + return; + } + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(INA238_CONVERSION_INTERVAL); + + } else { + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + + if (init() != PX4_OK) { + ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US); + } + } +} + +void INA238::print_status() +{ + I2CSPIDriverBase::print_status(); + + if (_initialized) { + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + printf("poll interval: %u \n", _measure_interval); + + } else { + PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.", + INA238_INIT_RETRY_INTERVAL_US / 1000); + } +} diff --git a/src/drivers/power_monitor/ina238/ina238.h b/src/drivers/power_monitor/ina238/ina238.h new file mode 100644 index 000000000000..a9108c258eab --- /dev/null +++ b/src/drivers/power_monitor/ina238/ina238.h @@ -0,0 +1,365 @@ +/**************************************************************************** + * + * 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 + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +/* Configuration Constants */ +#define INA238_BASEADDR 0x45 /* 7-bit address. 8-bit address is 0x45 */ +// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to +// connect to the INA238 every this many microseconds +#define INA238_INIT_RETRY_INTERVAL_US 500000 + +/* INA238 Registers addresses */ +#define INA238_REG_CONFIG (0x00) +#define INA238_REG_ADCCONFIG (0x01) +#define INA238_REG_SHUNTCAL (0x02) +#define INA238_REG_SHUNTTEMPCO (0x03) +#define INA238_REG_VSHUNT (0x04) +#define INA238_REG_VSBUS (0x05) +#define INA238_REG_DIETEMP (0x06) +#define INA238_REG_CURRENT (0x07) +#define INA238_REG_POWER (0x08) +#define INA238_REG_ENERGY (0x09) +#define INA238_REG_CHARGE (0x0a) +#define INA238_REG_DIAG_ALRT (0x0b) +#define INA238_REG_SOVL (0x0c) +#define INA238_REG_SUVL (0x0d) +#define INA238_REG_BOVL (0x0e) +#define INA238_REG_BUVL (0x0f) +#define INA238_REG_TEMP_LIMIT (0x10) +#define INA238_REG_TPWR_LIMIT (0x11) +#define INA238_MANUFACTURER_ID (0x3e) +#define INA238_DEVICE_ID (0x3f) + +#define INA238_MFG_ID_TI (0x5449) // TI +#define INA238_MFG_DIE (0x238) // INA237, INA238 + +/* INA238 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */ +#define INA238_ADCRANGE_SHIFTS (4) +#define INA238_ADCRANGE_MASK (1 << INA238_ADCRANGE_SHIFTS) +#define INA238_ADCRANGE_LOW (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV +#define INA238_ADCRANGE_HIGH (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV +#define INA238_TEMPCOMP_SHIFTS (5) +#define INA238_TEMPCOMP_MASK (1 << INA238_TEMPCOMP_SHIFTS) +#define INA238_TEMPCOMP_ENABLE (1 << INA238_TEMPCOMP_SHIFTS) +#define INA238_TEMPCOMP_DISABLE (0 << INA238_TEMPCOMP_SHIFTS) + +#define INA238_CONVDLY_SHIFTS (6) +#define INA238_CONVDLY_MASK (0xff << INA238_CONVDLY_SHIFTS) +#define INA238_CONVDLY2MS(n) ((n) << INA238_CONVDLY_SHIFTS) + +#define INA238_RSTACC_SHIFTS (14) +#define INA238_RSTACC_MASK (1 << INA238_RSTACC_SHIFTS) +#define INA238_RSTACC_CLEAR (1 << INA238_RSTACC_SHIFTS) +#define INA238_RSTACC_NORMAL (0 << INA238_RSTACC_SHIFTS) + +#define INA238_RST_SHIFTS (15) +#define INA238_RST_MASK (1 << INA238_RST_SHIFTS) +#define INA238_RST_RESET (1 << INA238_RST_SHIFTS) +#define INA238_RST_NORMAL (0 << INA238_RST_SHIFTS) + +/* INA238 ADC Configuration (ADC_CONFIG) 16-bit Register (Address = 1h) [reset = FB68h] */ +#define INA238_MODE_SHIFTS (12) +#define INA238_MODE_MASK (0xf << INA238_MODE_SHIFTS) +#define INA238_MODE_SHUTDOWN_TRIG (0 << INA238_MODE_SHIFTS) +#define INA238_MODE_BUS_TRIG (1 << INA238_MODE_SHIFTS) +#define INA238_MODE_SHUNT_TRIG (2 << INA238_MODE_SHIFTS) +#define INA238_MODE_SHUNT_BUS_TRIG (3 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_TRIG (4 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_BUS_TRIG (5 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_SHUNT_TRIG (6 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_SHUNT_BUS_TRIG (7 << INA238_MODE_SHIFTS) + +#define INA238_MODE_SHUTDOWN_CONT (8 << INA238_MODE_SHIFTS) +#define INA238_MODE_BUS_CONT (9 << INA238_MODE_SHIFTS) +#define INA238_MODE_SHUNT_CONT (10 << INA238_MODE_SHIFTS) +#define INA238_MODE_SHUNT_BUS_CONT (11 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_CONT (12 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_BUS_CONT (13 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_SHUNT_CONT (14 << INA238_MODE_SHIFTS) +#define INA238_MODE_TEMP_SHUNT_BUS_CONT (15 << INA238_MODE_SHIFTS) + +#define INA238_VBUSCT_SHIFTS (9) +#define INA238_VBUSCT_MASK (7 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_50US (0 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_84US (1 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_150US (2 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_280US (3 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_540US (4 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_1052US (5 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_2074US (6 << INA238_VBUSCT_SHIFTS) +#define INA238_VBUSCT_4170US (7 << INA238_VBUSCT_SHIFTS) + +#define INA238_VSHCT_SHIFTS (6) +#define INA238_VSHCT_MASK (7 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_50US (0 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_84US (1 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_150US (2 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_280US (3 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_540US (4 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_1052US (5 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_2074US (6 << INA238_VSHCT_SHIFTS) +#define INA238_VSHCT_4170US (7 << INA238_VSHCT_SHIFTS) + +#define INA238_VTCT_SHIFTS (3) +#define INA238_VTCT_MASK (7 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_50US (0 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_84US (1 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_150US (2 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_280US (3 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_540US (4 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_1052US (5 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_2074US (6 << INA238_VTCT_SHIFTS) +#define INA238_VTCT_4170US (7 << INA238_VTCT_SHIFTS) + +#define INA238_AVERAGES_SHIFTS (0) +#define INA238_AVERAGES_MASK (7 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_1 (0 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_4 (1 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_16 (2 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_64 (3 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_128 (4 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_256 (5 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_512 (6 << INA238_AVERAGES_SHIFTS) +#define INA238_AVERAGES_1024 (7 << INA238_AVERAGES_SHIFTS) + +#define INA238_ADCCONFIG (INA238_MODE_TEMP_SHUNT_BUS_CONT | INA238_VBUSCT_540US | INA238_VSHCT_540US | INA238_VTCT_540US |INA238_AVERAGES_64) + +/* INA238 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */ + +#define INA238_CURRLSB_SHIFTS (0) +#define INA238_CURRLSB_MASK (0x7fff << INA238_CURRLSB_SHIFTS) + +/* INA238 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */ + +#define INA238_TEMPCO_SHIFTS (0) +#define INA238_TEMPCO_MASK (0x1fff << INA238_TEMPCO_SHIFTS) + +/* INA238 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */ + +#define INA238_VSHUNT_SHIFTS (4) +#define INA238_VSHUNT_MASK (UINT32_C(0xffffff) << INA238_VSHUNT_SHIFTS) + +/* INA238 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */ + +#define INA238_VBUS_SHIFTS (4) +#define INA238_VBUS_MASK (UINT32_C(0xffffff) << INA238_VBUS_SHIFTS) + +/* INA238 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */ + +#define INA238_DIETEMP_SHIFTS (0) +#define INA238_DIETEMP_MASK (0xffff << INA238_DIETEMP_SHIFTS) + +/* INA238 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */ + +#define INA238_CURRENT_SHIFTS (4) +#define INA238_CURRENT_MASK (UINT32_C(0xffffff) << INA238_CURRENT_SHIFTS) + +/* INA238 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */ + +#define INA238_POWER_SHIFTS (0) +#define INA238_POWER_MASK (UINT32_C(0xffffff) << INA238_POWER_SHIFTS) + +/* INA238 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */ + +#define INA238_ENERGY_SHIFTS (0) +#define INA238_ENERGY_MASK (UINT64_C(0xffffffffff) << INA238_ENERGY_SHIFTS) + +/* INA238 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ + +#define INA238_CHARGE_SHIFTS (0) +#define INA238_CHARGE_MASK (UINT64_C(0xffffffffff) << INA238_CHARGE_SHIFTS) + + +/* INA238 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */ + +#define INA238_MEMSTAT (1 << 0) // This bit is set to 0 if a checksum error is detected in the device trim memory space +#define INA238_CNVRF (1 << 1) // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion. +#define INA238_POL (1 << 2) // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register. +#define INA238_BUSUL (1 << 3) // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register. +#define INA238_BUSOL (1 << 4) // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register. +#define INA238_SHNTUL (1 << 5) // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register +#define INA238_SHNTOL (1 << 6) // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register. +#define INA238_TMPOL (1 << 7) // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register. +#define INA238_MATHOF (1 << 9) // This bit is set to 1 if an arithmetic operation resulted in an overflow error. +#define INA238_CHARGEOF (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1. +#define INA238_ENERGYOF (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1. +#define INA238_APOL (1 << 12) // Alert Polarity bit sets the Alert pin polarity. +#define INA238_SLOWALER (1 << 13) // ALERT function is asserted on the completed averaged value. This gives the flexibility to delay the ALERT after the averaged value. +#define INA238_CNVR (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed +#define INA238_ALATCH (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been +// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until +// the DIAG_ALRT Register has been read. + +/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */ + +#define INA238_SOVL_SHIFTS (0) +#define INA238_SOVL_MASK (0xffff << INA238_SOVL_SHIFTS) + +/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */ + +#define INA238_SUVL_SHIFTS (0) +#define INA238_SUVL_MASK (0xffff << INA238_SUVL_SHIFTS) + +/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */ + +#define INA238_BOVL_SHIFTS (0) +#define INA238_BOVL_MASK (0xffff << INA238_BOVL_SHIFTS) + +/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */ + +#define INA238_BUVL_SHIFTS (0) +#define INA238_BUVL_MASK (0xffff << INA238_BUVL_SHIFTS) + +/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */ + +#define INA238_TEMP_LIMIT_SHIFTS (0) +#define INA238_TEMP_LIMIT_MASK (0xffff << INA238_TEMP_LIMIT_SHIFTS) + +/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */ + +#define INA238_POWER_LIMIT_SHIFTS (0) +#define INA238_POWER_LIMIT_MASK (0xffff << INA238_POWER_LIMIT_SHIFTS) + +/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */ + +/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2380h] */ + +#define INA238_DEVICE_REV_ID_SHIFTS (0) +#define INA238_DEVICE_REV_ID_MASK (0xf << INA238_DEVICE_REV_ID_SHIFTS) +#define INA238_DEVICEREV_ID(v) (((v) & INA238_DEVICE_REV_ID_MASK) >> INA238_DEVICE_REV_ID_SHIFTS) +#define INA238_DEVICE_ID_SHIFTS (4) +#define INA238_DEVICE_ID_MASK (0xfff << INA238_DEVICE_ID_SHIFTS) +#define INA238_DEVICEID(v) (((v) & INA238_DEVICE_ID_MASK) >> INA238_DEVICE_ID_SHIFTS) + +#define INA238_SAMPLE_FREQUENCY_HZ 10 +#define INA238_SAMPLE_INTERVAL_US (1_s / INA238_SAMPLE_FREQUENCY_HZ) +#define INA238_CONVERSION_INTERVAL (INA238_SAMPLE_INTERVAL_US - 7) +#define INA238_DN_MAX 32768.0f /* 2^15 */ +#define INA238_CONST 819.2e6f /* is an internal fixed value used to ensure scaling is maintained properly */ +#define INA238_VSCALE 3.125e-03f /* LSB of voltage is 3.1255 mV/LSB */ + + +#define DEFAULT_MAX_CURRENT 327.68f /* Amps */ +#define DEFAULT_SHUNT 0.0003f /* Shunt is 300 uOhm */ + +#define swap16(w) __builtin_bswap16((w)) +#define swap32(d) __builtin_bswap32((d)) +#define swap64(q) __builtin_bswap64((q)) + +class INA238 : public device::I2C, public ModuleParams, public I2CSPIDriver +{ +public: + INA238(const I2CSPIDriverConfig &config, int battery_index); + virtual ~INA238(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + + /** + * Tries to call the init() function. If it fails, then it will schedule to retry again in + * INA238_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds. + * + * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise. + */ + int force_init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_status() override; + +protected: + int probe() override; + +private: + bool _sensor_ok{false}; + unsigned int _measure_interval{0}; + bool _collect_phase{false}; + bool _initialized{false}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _collection_errors; + + // Configuration state, computed from params + float _max_current; + float _rshunt; + float _current_lsb; + int16_t _range; + + Battery _battery; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + int read(uint8_t address, uint16_t &data); + int write(uint8_t address, uint16_t data); + + int read(uint8_t address, int16_t &data) + { + return read(address, (uint16_t &)data); + } + + int write(uint8_t address, int16_t data) + { + return write(address, (uint16_t)data); + } + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + int collect(); + +}; diff --git a/src/drivers/power_monitor/ina238/ina238_main.cpp b/src/drivers/power_monitor/ina238/ina238_main.cpp new file mode 100644 index 000000000000..879d0a0cf342 --- /dev/null +++ b/src/drivers/power_monitor/ina238/ina238_main.cpp @@ -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. + * + ****************************************************************************/ +#include +#include + +#include "ina238.h" + +I2CSPIDriverBase *INA238::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + INA238 *instance = new INA238(config, config.custom1); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (config.keep_running) { + if (instance->force_init() != PX4_OK) { + PX4_INFO("Failed to init INA238 on bus %d, but will try again periodically.", config.bus); + } + + } else if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + return instance; +} + +void +INA238::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for the INA238 power monitor. + +Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address. + +For example, one instance can run on Bus 2, address 0x45, and one can run on Bus 2, address 0x45. + +If the INA238 module is not powered, then by default, initialization of the driver will fail. To change this, use +the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again +every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without +this flag set, the battery must be plugged in before starting the driver. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ina238", "driver"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x45); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); + PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int +ina238_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = INA238; + BusCLIArguments cli{true, false}; + cli.i2c_address = INA238_BASEADDR; + cli.default_i2c_frequency = 100000; + cli.support_keep_running = true; + cli.custom1 = 1; + + while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) { + switch (ch) { + case 't': // battery index + cli.custom1 = (int)strtol(cli.optArg(), NULL, 0); + break; + } + } + + const char *verb = cli.optArg(); + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA238); + + 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/power_monitor/ina238/ina238_params.c b/src/drivers/power_monitor/ina238/ina238_params.c new file mode 100644 index 000000000000..585cb0151209 --- /dev/null +++ b/src/drivers/power_monitor/ina238/ina238_params.c @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Enable INA238 Power Monitor + * + * For systems a INA238 Power Monitor, this should be set to true + * + * @group Sensors + * @boolean + * @reboot_required true +*/ +PARAM_DEFINE_INT32(SENS_EN_INA238, 0); + +/** + * INA238 Power Monitor Max Current + * + * @group Sensors + * @min 0.1 + * @max 327.68 + * @decimal 2 + * @increment 0.1 + */ +PARAM_DEFINE_FLOAT(INA238_CURRENT, 327.68f); + +/** + * INA238 Power Monitor Shunt + * + * @group Sensors + * @min 0.000000001 + * @max 0.1 + * @decimal 10 + * @increment .000000001 + */ +PARAM_DEFINE_FLOAT(INA238_SHUNT, 0.0003f); diff --git a/src/drivers/power_monitor/voxlpm/Kconfig b/src/drivers/power_monitor/voxlpm/Kconfig new file mode 100644 index 000000000000..f2553b1ed020 --- /dev/null +++ b/src/drivers/power_monitor/voxlpm/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_VOXLPM + bool "voxlpm" + default n + ---help--- + Enable support for voxlpm \ No newline at end of file diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 84b2be3ee637..91270538b337 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -47,14 +47,14 @@ * Address 0x44 - measures battery voltage and current with a 0.0005 ohm sense resistor * Address 0x45 - measures 5VDC/12VDC ouptut voltage and current with a 0.005 ohm sense resistor */ -VOXLPM::VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type) : - I2C(DRV_POWER_DEVTYPE_VOXLPM, MODULE_NAME, bus, VOXLPM_INA231_ADDR_VBATT, bus_frequency), +VOXLPM::VOXLPM(const I2CSPIDriverConfig &config) : + I2C(config), ModuleParams(nullptr), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + I2CSPIDriver(config), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), - _ch_type(ch_type), - _battery(1, this, _meas_interval_us) + _ch_type((VOXLPM_CH_TYPE)config.custom1), + _battery(1, this, _meas_interval_us, battery_status_s::BATTERY_SOURCE_POWER_MODULE) { } @@ -71,15 +71,10 @@ VOXLPM::init() int ret = PX4_ERROR; if (_ch_type == VOXLPM_CH_TYPE_VBATT) { - _battery.updateBatteryStatus( - hrt_absolute_time(), - 0.0, - 0.0, - false, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0 - ); + _battery.setConnected(false); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } /* do I2C init, it will probe the bus for two possible configurations, LTC2946 or INA231 */ @@ -346,15 +341,11 @@ VOXLPM::measure() if (ret == PX4_OK) { switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _actuators_sub.copy(&_actuator_controls); - - _battery.updateBatteryStatus(tnow, - _voltage, - _amperage, - true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]); + + _battery.setConnected(true); + _battery.updateVoltage(_voltage); + _battery.updateCurrent(_amperage); + _battery.updateAndPublishBatteryStatus(tnow); } // fallthrough @@ -377,13 +368,10 @@ VOXLPM::measure() switch (_ch_type) { case VOXLPM_CH_TYPE_VBATT: { - _battery.updateBatteryStatus(tnow, - 0.0, - 0.0, - true, - battery_status_s::BATTERY_SOURCE_POWER_MODULE, - 0, - 0.0); + _battery.setConnected(true); + _battery.updateVoltage(0.f); + _battery.updateCurrent(0.f); + _battery.updateAndPublishBatteryStatus(tnow); } break; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.hpp b/src/drivers/power_monitor/voxlpm/voxlpm.hpp index 5717542ca83a..e03326d62297 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.hpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.hpp @@ -88,9 +88,7 @@ #include #include -#include #include -#include #include #include #include @@ -225,11 +223,10 @@ enum VOXLPM_CH_TYPE { class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - VOXLPM(I2CSPIBusOption bus_option, const int bus, int bus_frequency, VOXLPM_CH_TYPE ch_type); + VOXLPM(const I2CSPIDriverConfig &config); virtual ~VOXLPM(); - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); static void print_usage(); virtual int init(); @@ -268,8 +265,6 @@ class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriverforce_init()) { - PX4_INFO("Failed to init voxlpm type: %d on bus: %d, but will try again periodically.", (VOXLPM_CH_TYPE)cli.type, - iterator.bus()); + PX4_INFO("Failed to init voxlpm type: %d on bus: %d, but will try again periodically.", config.custom1, config.bus); } } else if (OK != instance->init()) { @@ -68,6 +65,7 @@ VOXLPM::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x44); PRINT_MODULE_USAGE_PARAM_STRING('T', "VBATT", "VBATT|P5VDC|P12VDC", "Type", true); PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -80,20 +78,21 @@ voxlpm_main(int argc, char *argv[]) using ThisDriver = VOXLPM; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = 400000; - cli.type = VOXLPM_CH_TYPE_VBATT; + cli.custom1 = VOXLPM_CH_TYPE_VBATT; cli.support_keep_running = true; + cli.i2c_address = VOXLPM_INA231_ADDR_VBATT; - while ((ch = cli.getopt(argc, argv, "T:")) != EOF) { + while ((ch = cli.getOpt(argc, argv, "T:")) != EOF) { switch (ch) { case 'T': - if (strcmp(cli.optarg(), "VBATT") == 0) { - cli.type = VOXLPM_CH_TYPE_VBATT; + if (strcmp(cli.optArg(), "VBATT") == 0) { + cli.custom1 = VOXLPM_CH_TYPE_VBATT; - } else if (strcmp(cli.optarg(), "P5VDC") == 0) { - cli.type = VOXLPM_CH_TYPE_P5VDC; + } else if (strcmp(cli.optArg(), "P5VDC") == 0) { + cli.custom1 = VOXLPM_CH_TYPE_P5VDC; - } else if (strcmp(cli.optarg(), "P12VDC") == 0) { - cli.type = VOXLPM_CH_TYPE_P12VDC; // same as P5VDC + } else if (strcmp(cli.optArg(), "P12VDC") == 0) { + cli.custom1 = VOXLPM_CH_TYPE_P12VDC; // same as P5VDC } else { PX4_ERR("unknown type"); @@ -104,7 +103,7 @@ voxlpm_main(int argc, char *argv[]) } } - const char *verb = cli.optarg(); + const char *verb = cli.optArg(); if (!verb) { ThisDriver::print_usage(); diff --git a/src/drivers/pps_capture/CMakeLists.txt b/src/drivers/pps_capture/CMakeLists.txt new file mode 100644 index 000000000000..cf613393f68c --- /dev/null +++ b/src/drivers/pps_capture/CMakeLists.txt @@ -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. +# +############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + +px4_add_module( + MODULE drivers__pps_capture + MAIN pps_capture + COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" + SRCS + PPSCapture.cpp + ) diff --git a/src/drivers/pps_capture/Kconfig b/src/drivers/pps_capture/Kconfig new file mode 100644 index 000000000000..9a41a5eae3ee --- /dev/null +++ b/src/drivers/pps_capture/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PPS_CAPTURE + bool "pps_capture" + default n + ---help--- + Enable support for pps_capture \ No newline at end of file diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp new file mode 100644 index 000000000000..aeb8eb0e8d6f --- /dev/null +++ b/src/drivers/pps_capture/PPSCapture.cpp @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * 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 PPSCapture.cpp + * + * This is driver for capturing GNSS Pulse Per Second (PPS) signal. + * + */ + +#include "PPSCapture.hpp" +#include +#include +#include + +PPSCapture::PPSCapture() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ + _pps_capture_pub.advertise(); +} + +PPSCapture::~PPSCapture() +{ + if (_channel >= 0) { + io_timer_unallocate_channel(_channel); + px4_arch_gpiosetevent(_pps_capture_gpio, false, false, false, nullptr, nullptr); + } +} + +bool PPSCapture::init() +{ + bool success = false; + + param_t p_ctrl_alloc = param_find("SYS_CTRL_ALLOC"); + int32_t ctrl_alloc = 0; + + if (p_ctrl_alloc != PARAM_INVALID) { + param_get(p_ctrl_alloc, &ctrl_alloc); + } + + if (ctrl_alloc == 1) { + + for (unsigned i = 0; i < 16; ++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 + _channel = i; + } + } + } + } + +#if defined(PPS_CAPTURE_CHANNEL) + + if (_channel == -1) { + _channel = PPS_CAPTURE_CHANNEL; + } + +#endif + + if (_channel == -1) { + PX4_WARN("No pps channel configured"); + return false; + } + + int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_PPS); + + if (ret != PX4_OK) { + PX4_ERR("gpio alloc failed (%i) for PPS at channel (%d)", ret, _channel); + return false; + } + + _pps_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel)); + int ret_val = px4_arch_gpiosetevent(_pps_capture_gpio, true, false, true, &PPSCapture::gpio_interrupt_callback, this); + + if (ret_val == PX4_OK) { + success = true; + } + + return success; +} + +void PPSCapture::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + sensor_gps_s sensor_gps; + + if (_sensor_gps_sub.update(&sensor_gps)) { + _last_gps_utc_timestamp = sensor_gps.time_utc_usec; + _last_gps_timestamp = sensor_gps.timestamp; + } + + pps_capture_s pps_capture; + pps_capture.timestamp = _hrt_timestamp; + // GPS UTC time when the GPIO interrupt was triggered + // Last UTC time received from the GPS + elapsed time to the PPS interrupt + uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp); + + // (For ubx F9P) The rising edge of the PPS pulse is aligned to the top of second GPS time base. + // So, remove the fraction of second and shift to the next second. The interrupt is triggered + // before the matching timestamp is received via a UART message, which means the last received GPS time is always + // behind. + pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC; + + _pps_capture_pub.publish(pps_capture); +} + +int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg) +{ + PPSCapture *instance = static_cast(arg); + + instance->_hrt_timestamp = hrt_absolute_time(); + instance->ScheduleNow(); // schedule work queue to publish PPS captured time + + return PX4_OK; +} + +int PPSCapture::task_spawn(int argc, char *argv[]) +{ + PPSCapture *instance = new PPSCapture(); + + 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 PPSCapture::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int PPSCapture::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements capturing PPS information from the GNSS module and calculates the drift between PPS and Real-time clock. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pps_capture", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +void PPSCapture::stop() +{ + exit_and_cleanup(); +} + +extern "C" __EXPORT int pps_capture_main(int argc, char *argv[]) +{ + if (argc >= 2 && !strcmp(argv[1], "stop") && PPSCapture::is_running()) { + PPSCapture::stop(); + } + + return PPSCapture::main(argc, argv); +} diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp new file mode 100644 index 000000000000..cb29c842dac2 --- /dev/null +++ b/src/drivers/pps_capture/PPSCapture.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class PPSCapture : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + PPSCapture(); + virtual ~PPSCapture(); + + /** @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); + + /** PPSCapture is an interrupt-driven task and needs to be manually stopped */ + static void stop(); + +private: + void Run() override; + + int _channel{-1}; + uint32_t _pps_capture_gpio{0}; + uORB::Publication _pps_capture_pub{ORB_ID(pps_capture)}; + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; + + hrt_abstime _hrt_timestamp{0}; + + hrt_abstime _last_gps_timestamp{0}; + uint64_t _last_gps_utc_timestamp{0}; + +}; diff --git a/src/drivers/pps_capture/pps_capture_params.c b/src/drivers/pps_capture/pps_capture_params.c new file mode 100644 index 000000000000..68442ec83918 --- /dev/null +++ b/src/drivers/pps_capture/pps_capture_params.c @@ -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. + * + ****************************************************************************/ + +/** + * @file pps_capture_params.c + * PPS Capture params + */ +/** + * PPS Capture Enable + * + * Enables the PPS capture module. + * This switches mode of FMU channel 7 to be the + * PPS input channel. + * + * @boolean + * @group GPS + * @reboot_required true + */ +PARAM_DEFINE_INT32(PPS_CAP_ENABLE, 0); diff --git a/src/drivers/protocol_splitter/CMakeLists.txt b/src/drivers/protocol_splitter/CMakeLists.txt index de6865adf601..8e09f1b10645 100644 --- a/src/drivers/protocol_splitter/CMakeLists.txt +++ b/src/drivers/protocol_splitter/CMakeLists.txt @@ -37,4 +37,3 @@ px4_add_module( protocol_splitter.cpp DEPENDS ) - diff --git a/src/drivers/protocol_splitter/Kconfig b/src/drivers/protocol_splitter/Kconfig new file mode 100644 index 000000000000..f251603986a1 --- /dev/null +++ b/src/drivers/protocol_splitter/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PROTOCOL_SPLITTER + bool "protocol_splitter" + default n + ---help--- + Enable support for protocol_splitter \ No newline at end of file diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp index ed25867f6eff..dacf0bb4c504 100644 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ b/src/drivers/protocol_splitter/protocol_splitter.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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,8 +33,11 @@ /** * @file protocol_splitter.cpp - * NuttX Driver to multiplex mavlink and RTPS on a single serial port. - * Makes sure the two protocols can be read & written simultanously by 2 processes. + * + * NuttX Driver to multiplex MAVLink and RTPS on a single serial port. + * Makes sure the two protocols can be read & written simultaneously by two + * processes. + * * It will create two devices: * /dev/mavlink * /dev/rtps @@ -43,11 +46,17 @@ #include #include #include +#include +#include #include +#include #include +#include +#include #include -#include +#include + class Mavlink2Dev; class RtpsDev; @@ -55,9 +64,37 @@ class ReadBuffer; extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); - -const char *Sp2HeaderMagic = "SP2"; -const int Sp2HeaderSize = 8; +/* + MessageType is in MSB of header[1] + | + v + Mavlink 0000 0000b + Rtps 1000 0000b +*/ +enum class MessageType : uint8_t {Mavlink = 0x00, Rtps = 0x01}; + +constexpr char Sp2HeaderMagic = 'S'; +constexpr int Sp2HeaderSize = 4; + +/* + Header Structure: + + bits: 1 2 3 4 5 6 7 8 + header[0] - | Magic | + header[1] - |T| LenH | + header[2] - | LenL | + header[3] - | Checksum | +*/ +union __attribute__((packed)) Sp2Header { + uint8_t bytes[4]; + struct { + char magic; // 'S' + uint8_t len_h: 7, // Length MSB + type: 1; // 0=MAVLINK, 1=RTPS + uint8_t len_l; // Length LSB + uint8_t checksum; // XOR of the three bytes above + } fields; +}; struct StaticData { Mavlink2Dev *mavlink2; @@ -73,6 +110,16 @@ namespace static StaticData *objects = nullptr; } +// Perf counters +perf_counter_t bytes_received_count; +perf_counter_t header_bytes_received_count; +perf_counter_t bytes_lost_count; +perf_counter_t mavlink_messages_parsed_count; +perf_counter_t mavlink_bytes_parsed_count; +perf_counter_t rtps_messages_parsed_count; +perf_counter_t rtps_bytes_parsed_count; +perf_counter_t buffer_drops; + class ReadBuffer { public: @@ -80,33 +127,71 @@ class ReadBuffer void copy(void *dest, size_t pos, size_t n); void remove(size_t pos, size_t n); - uint8_t buffer[512] = {}; + void print_stats(); + void update_lost_stats(); + + uint8_t buffer[1024] = {}; size_t buf_size = 0; static const size_t BUFFER_THRESHOLD = sizeof(buffer) * 0.8; + + // We keep track of the first Mavlink and Rtps packet in the buffer. + // If start and end are equal there is no packet. + size_t start_mavlink = 0; + size_t end_mavlink = 0; + size_t start_rtps = 0; + size_t end_rtps = 0; + // Just for stats. + size_t mavlink_parsed = 0; + size_t rtps_parsed = 0; + size_t bytes_received = 0; + size_t bytes_lost = 0; + size_t header_bytes_received = 0; }; int ReadBuffer::read(int fd) { - /* Discard whole buffer if it's filled beyond a threshold, - * This should prevent buffer being filled by garbage that - * no reader (MAVLink or RTPS) can understand. - * - * TODO: a better approach would be checking if both reader - * start understanding messages beyond a certain buffer size, - * meaning that everything before is garbage. - */ if (buf_size > BUFFER_THRESHOLD) { - buf_size = 0; + // Drop the buffer if it's too full. This is not expected to happen, but it might, if one of the readers + // is too slow. In that case it's best to make space for new data, otherwise the faster reader might + // busy-loop w/o getting new data. + perf_count(buffer_drops); + + PX4_DEBUG("Buffer full, dropping: %zu %zu %zu %zu", start_mavlink, end_mavlink, start_rtps, end_rtps); + + // Drop only as much as we have to + size_t num_remove = math::max(start_mavlink, start_rtps); + + if (num_remove == 0) { + num_remove = buf_size; + } + + remove(0, num_remove); + start_mavlink -= math::min(num_remove, start_mavlink); + end_mavlink -= math::min(num_remove, end_mavlink); + start_rtps -= math::min(num_remove, start_rtps); + end_rtps -= math::min(num_remove, end_rtps); } - int r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); + int bytes_available = 0; + int err = ::ioctl(fd, FIONREAD, (unsigned long)&bytes_available); + ssize_t r = 0; + + if (err != 0 || bytes_available > 0) { + r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); + } - if (r < 0) { + if (r <= 0) { return r; } buf_size += r; + bytes_received += r; + + // Update the lost/unused bytes count + update_lost_stats(); + + perf_set_count(bytes_received_count, bytes_received); return r; } @@ -117,7 +202,7 @@ void ReadBuffer::copy(void *dest, size_t pos, size_t n) ASSERT(pos + n <= buf_size); if (dest) { - memmove(dest, buffer + pos, n); // send desired data + memcpy(dest, buffer + pos, n); } } @@ -126,14 +211,57 @@ void ReadBuffer::remove(size_t pos, size_t n) ASSERT(pos < buf_size); ASSERT(pos + n <= buf_size); - memmove(buffer + pos, buffer + (pos + n), sizeof(buffer) - pos - n); + memmove(buffer + pos, buffer + (pos + n), buf_size - pos - n); buf_size -= n; } +void ReadBuffer::update_lost_stats() +{ + bytes_lost = bytes_received - mavlink_parsed - rtps_parsed - header_bytes_received; + + if (end_mavlink > start_mavlink) { + bytes_lost -= end_mavlink - start_mavlink; + } + + if (end_rtps > start_rtps) { + bytes_lost -= end_rtps - start_rtps; + } + + perf_set_count(bytes_lost_count, bytes_lost); +} + +void ReadBuffer::print_stats() +{ + PX4_INFO_RAW("\tReceived:\n"); + PX4_INFO_RAW("\tTotal: %9zu bytes\n", + bytes_received); + PX4_INFO_RAW("\tHeaders: %9zu bytes (%5.1f %%)\n", + header_bytes_received, + static_cast(static_cast(header_bytes_received) + / static_cast(bytes_received) + * 100.f)); + PX4_INFO_RAW("\tMAVLink: %9zu bytes (%5.1f %%)\n", + mavlink_parsed, + static_cast(static_cast(mavlink_parsed) + / static_cast(bytes_received - header_bytes_received) + * 100.f)); + PX4_INFO_RAW("\tRTPS: %9zu bytes (%5.1f %%)\n", + rtps_parsed, + static_cast(static_cast(rtps_parsed) + / static_cast(bytes_received - header_bytes_received) + * 100.f)); + + PX4_INFO_RAW("\tUnused: %9zu bytes (%5.1f %%)\n", bytes_lost, + static_cast(static_cast(bytes_lost) + / static_cast(bytes_received) + * 100.f)); +} + + class DevCommon : public cdev::CDev { public: - DevCommon(const char *device_path); + DevCommon(const char *device_path, ReadBuffer *read_buffer); virtual ~DevCommon(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); @@ -145,21 +273,11 @@ class DevCommon : public cdev::CDev protected: - /* - struct Sp2Header { - char magic[3]; - uint8_t type; - uint16_t payload_len; - uint16_t reserved (align) - } - */ - - enum MessageType {Mavlink = 0, Rtps}; - - uint8_t _header[8] = {}; - virtual pollevent_t poll_state(struct file *filp); + int try_to_copy_data(char *buffer, size_t buflen, MessageType message_type); + void scan_for_packets(); + void cleanup(); void lock(enum Operation op) { @@ -181,37 +299,41 @@ class DevCommon : public cdev::CDev int _fd = -1; - uint16_t _packet_len; - enum class ParserState : uint8_t { - Idle = 0, - GotLength - }; - ParserState _parser_state = ParserState::Idle; - - bool _had_data = false; ///< whether poll() returned available data + Sp2Header _header; -private: + ReadBuffer *_read_buffer; }; -DevCommon::DevCommon(const char *device_path) +DevCommon::DevCommon(const char *device_path, ReadBuffer *read_buffer) : CDev(device_path) + , _read_buffer(read_buffer) { } DevCommon::~DevCommon() { if (_fd >= 0) { + // discard all pending data, as close() might block otherwise on NuttX with flow control enabled + tcflush(_fd, TCIOFLUSH); ::close(_fd); } } int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg) { - //pretend we have enough space left to write, so mavlink will not drop data and throw off - //our parsing state if (cmd == FIONSPACE) { - *(int *)arg = 1024; - return 0; + int ret = ::ioctl(_fd, FIONSPACE, arg); + + if (ret == 0) { + int *buf_free = (int *)arg; + *buf_free -= sizeof(_header); + + if (*buf_free < 0) { + *buf_free = 0; + } + } + + return ret; } return ::ioctl(_fd, cmd, arg); @@ -220,8 +342,16 @@ int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg) int DevCommon::open(file *filp) { _fd = ::open(objects->device_name, O_RDWR | O_NOCTTY); + + if (_fd < 0) { + PX4_ERR("open failed: %s", strerror(errno)); + return -1; + } + CDev::open(filp); - return _fd >= 0 ? 0 : -1; + + + return 0; } int DevCommon::close(file *filp) @@ -247,335 +377,364 @@ pollevent_t DevCommon::poll_state(struct file *filp) * the _fd in here or by overriding some other method. */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 100); - _had_data = ret > 0 && (fds[0].revents & POLLIN); + ::poll(fds, sizeof(fds) / sizeof(fds[0]), 10); return POLLIN; } -class Mavlink2Dev : public DevCommon +int DevCommon::try_to_copy_data(char *buffer, size_t buflen, MessageType message_type) { -public: - Mavlink2Dev(ReadBuffer *_read_buffer); - virtual ~Mavlink2Dev() {} + if (buflen == 0) { + return 0; + } - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); + switch (message_type) { + case MessageType::Mavlink: + if (_read_buffer->start_mavlink < _read_buffer->end_mavlink) { + // We have Mavlink data ready to send. + const size_t len_available = _read_buffer->end_mavlink - _read_buffer->start_mavlink; + // We can only send what fits in the callers buffer. + const size_t len_to_copy = math::min(len_available, buflen); -protected: - ReadBuffer *_read_buffer; - size_t _remaining_partial = 0; - size_t _partial_start = 0; - uint8_t _partial_buffer[512] = {}; -}; + // Copy it to the callers buffer and remove it from our buffer. + _read_buffer->copy(buffer, _read_buffer->start_mavlink, len_to_copy); -Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) - : DevCommon("/dev/mavlink") - , _read_buffer{read_buffer} -{ - memcpy(_header, Sp2HeaderMagic, 3); - _header[3] = MessageType::Mavlink; - memset(&_header[4], 0, 4); -} + // Shift the markers accordingly. + _read_buffer->start_mavlink += len_to_copy; -ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) -{ - int i, ret; - uint16_t packet_len, payload_len; + // Keep track for stats. + _read_buffer->mavlink_parsed += len_to_copy; + + // Update the lost/unused bytes count + _read_buffer->update_lost_stats(); + + // Update the number of MAVLink bytes parsed + perf_set_count(mavlink_bytes_parsed_count, _read_buffer->mavlink_parsed); - /* last reading was partial (i.e., buffer didn't fit whole message), - * so now we'll just send remaining bytes */ - if (_remaining_partial > 0) { - size_t len = _remaining_partial; + // Update the number of MAVLink messages parsed + perf_count(mavlink_messages_parsed_count); - if (buflen < len) { - len = buflen; + return len_to_copy; + + } else { + return 0; } - memmove(buffer, _partial_buffer + _partial_start, len); - _partial_start += len; - _remaining_partial -= len; + case MessageType::Rtps: + if (_read_buffer->start_rtps < _read_buffer->end_rtps) { + // We have Rtps data ready to send + const size_t len_available = _read_buffer->end_rtps - _read_buffer->start_rtps; + // We can only send what fits in the callers buffer. + const size_t len_to_copy = math::min(len_available, buflen); + + // Copy it to the callers buffer and remove it from our buffer. + _read_buffer->copy(buffer, _read_buffer->start_rtps, len_to_copy); + + // Shift the markers accordingly. + _read_buffer->start_rtps += len_to_copy; + + // Keep track for stats. + _read_buffer->rtps_parsed += len_to_copy; - if (_remaining_partial == 0) { - _partial_start = 0; + // Update the lost/unused bytes count + _read_buffer->update_lost_stats(); + + // Update the number of RTPS bytes parsed + perf_set_count(rtps_bytes_parsed_count, _read_buffer->rtps_parsed); + + // Update the number of RTPS messages parsed + perf_count(rtps_messages_parsed_count); + + return len_to_copy; + + } else { + return 0; } - return len; - } + break; + - if (!_had_data) { + default: return 0; } +} - lock(Read); - ret = _read_buffer->read(_fd); - - if (ret < 0) { - goto end; +void DevCommon::scan_for_packets() +{ + if (_read_buffer->buf_size < Sp2HeaderSize) { + // We have not even one header in the buffer, no need to scan yet. + return; } - ret = 0; + bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); + bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); - if (_read_buffer->buf_size < Sp2HeaderSize) { - goto end; + if (mavlink_available && rtps_available) { + // We still have data for both, no need to scan yet. + return; } - // Search for a mavlink packet on buffer to send it - i = 0; + const size_t begin = math::min(_read_buffer->end_mavlink, _read_buffer->end_rtps); - while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && - (_read_buffer->buffer[i] != 'S' - || _read_buffer->buffer[i + 1] != 'P' - || _read_buffer->buffer[i + 2] != '2' - || _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Mavlink)) { - i++; - } + for (size_t i = begin; i < _read_buffer->buf_size - Sp2HeaderSize; /* ++i */) { - // We need at least the first six bytes to get packet len - if ((unsigned)i >= _read_buffer->buf_size - Sp2HeaderSize) { - goto end; - } + const Sp2Header *header = reinterpret_cast(&_read_buffer->buffer[i]); - payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5]; - packet_len = payload_len + Sp2HeaderSize; + if (header->fields.magic != Sp2HeaderMagic) { + // Not the magic byte that we're looking for. + ++i; + continue; + } - // packet is bigger than what we've read, better luck next time - if ((unsigned)i + packet_len > _read_buffer->buf_size) { - goto end; - } + const uint8_t checksum = (_read_buffer->buffer[i] ^ _read_buffer->buffer[i + 1] ^ _read_buffer->buffer[i + 2]); + + if (header->fields.checksum != checksum) { + // Checksum failed. + ++i; + continue; + } + + if (header->fields.type != static_cast(MessageType::Mavlink) && + header->fields.type != static_cast(MessageType::Rtps)) { + // Ignore unknown protocols + ++i; + continue; + } + + const size_t payload_len = ((uint16_t)header->fields.len_h << 8) | header->fields.len_l; + + if (payload_len > sizeof(_read_buffer->buffer)) { + // This can happen if by accident data matches the header including checksum. + // Given we skip most data using the last payload_len, we should not see this too often, + // unless the link is very lossy and we often have to re-sync. + PX4_DEBUG("payload size %zu > buffer size %zu: %d, protocol: %s", + payload_len, sizeof(_read_buffer->buffer), + (header->fields.type == static_cast(MessageType::Mavlink)) ? "Mavlink" : "Rtps"); + ++i; + continue; + } + + if (i + Sp2HeaderSize + payload_len > _read_buffer->buf_size) { + // We don't have a enough data in the buffer yet, try again later. + break; + } + + if (header->fields.type == static_cast(MessageType::Mavlink) && !mavlink_available) { + _read_buffer->start_mavlink = i + Sp2HeaderSize; + _read_buffer->end_mavlink = _read_buffer->start_mavlink + payload_len; + mavlink_available = true; + + // Overwrite header magic byte, so we don't parse them again. + _read_buffer->buffer[i] = 0; + _read_buffer->header_bytes_received += Sp2HeaderSize; + + } else if (header->fields.type == static_cast(MessageType::Rtps) && !rtps_available) { + _read_buffer->start_rtps = i + Sp2HeaderSize; + _read_buffer->end_rtps = _read_buffer->start_rtps + payload_len; + rtps_available = true; + + // Overwrite header magic byte, so we don't parse them again. + _read_buffer->buffer[i] = 0; + _read_buffer->header_bytes_received += Sp2HeaderSize; + } + + if (mavlink_available && rtps_available) { + // Both have at least one message ready, we can stop now. + break; + } + + // Update the lost/unused bytes count + _read_buffer->update_lost_stats(); + + perf_set_count(header_bytes_received_count, _read_buffer->header_bytes_received); + + i += Sp2HeaderSize + payload_len; - /* if buffer doesn't fit message, send what's possible and copy remaining - * data into a temporary buffer on this class */ - if (payload_len > buflen) { - _read_buffer->copy(buffer, i + Sp2HeaderSize, buflen); - _read_buffer->copy(_partial_buffer, i + Sp2HeaderSize + buflen, payload_len - buflen); - _read_buffer->remove(i, packet_len); - _remaining_partial = payload_len - buflen; - ret = buflen; - goto end; } +} - _read_buffer->copy(buffer, i + Sp2HeaderSize, payload_len); - _read_buffer->remove(i, packet_len); - ret = payload_len; +void DevCommon::cleanup() +{ + const bool mavlink_available = (_read_buffer->start_mavlink < _read_buffer->end_mavlink); + const bool rtps_available = (_read_buffer->start_rtps < _read_buffer->end_rtps); -end: - unlock(Read); - return ret; + // Clean up garbage bytes and accumulated headers + + size_t garbage_end = 0; + + if (!mavlink_available && !rtps_available) { + garbage_end = math::max(_read_buffer->start_mavlink, _read_buffer->start_rtps); + + } else { + garbage_end = math::min(_read_buffer->start_mavlink, _read_buffer->start_rtps); + } + + if (garbage_end > 0) { + _read_buffer->remove(0, garbage_end); + + _read_buffer->start_mavlink -= math::min(garbage_end, _read_buffer->start_mavlink); + _read_buffer->end_mavlink -= math::min(garbage_end, _read_buffer->end_mavlink); + _read_buffer->start_rtps -= math::min(garbage_end, _read_buffer->start_rtps); + _read_buffer->end_rtps -= math::min(garbage_end, _read_buffer->end_rtps); + } } -ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) + +class Mavlink2Dev : public DevCommon { - /* - * we need to look into the data to make sure the output is locked for the duration - * of a whole packet. - * assumptions: - * - packet header is written all at once (or at least it contains the payload length) - * - a single write call does not contain multiple (or parts of multiple) packets - */ - ssize_t ret = 0; +public: + Mavlink2Dev(ReadBuffer *read_buffer); + virtual ~Mavlink2Dev() {} - switch (_parser_state) { - case ParserState::Idle: - ASSERT(buflen >= 3); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); +}; - if ((unsigned char)buffer[0] == 253) { - uint8_t payload_len = buffer[1]; - uint8_t incompat_flags = buffer[2]; - _packet_len = payload_len + 12; +Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) + : DevCommon("/dev/mavlink", read_buffer) +{ + _header.fields.magic = Sp2HeaderMagic; + _header.fields.len_h = 0; + _header.fields.len_l = 0; + _header.fields.checksum = 0; + _header.fields.type = static_cast(MessageType::Mavlink); +} - if (incompat_flags & 0x1) { //signing - _packet_len += 13; - } +ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) +{ + lock(Read); - _parser_state = ParserState::GotLength; - lock(Write); + // The cleanup needs to be right after a scan, so we don't clean up + // something that we haven't found yet. + scan_for_packets(); + cleanup(); - } else if ((unsigned char)buffer[0] == 254) { // mavlink 1 - uint8_t payload_len = buffer[1]; - _packet_len = payload_len + 8; + // If we have already a packet ready in the current buffer, we don't have + // to read and can grab data straightaway. + int ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - _parser_state = ParserState::GotLength; - lock(Write); + if (ret > 0) { + unlock(Read); + return ret; + } - } else { - PX4_ERR("parser error"); - return 0; - } + // Otherwise, we have to do a read. + ret = _read_buffer->read(_fd); - /* FALLTHROUGH */ + if (ret <= 0) { + unlock(Read); + return ret; + } - case ParserState::GotLength: { - _packet_len -= buflen; - int buf_free; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); + // Now we need to check again if there is data available. + scan_for_packets(); - if (buf_free < (int)buflen) { - //let write fail, to let mavlink know the buffer would overflow - //(this is because in the ioctl we pretend there is always enough space) - ret = -1; + // And try to copy it out. + ret = try_to_copy_data(buffer, buflen, MessageType::Mavlink); - } else { - _header[4] = (uint8_t)((buflen >> 8) & 0xff); - _header[5] = (uint8_t)(buflen & 0xff); - ::write(_fd, _header, 8); - ret = ::write(_fd, buffer, buflen); - } + unlock(Read); + return ret; +} - if (_packet_len == 0) { - unlock(Write); - _parser_state = ParserState::Idle; - } +ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) +{ + _header.fields.len_h = (buflen >> 8) & 0x7f; + _header.fields.len_l = buflen & 0xff; + _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; + lock(Write); + int buf_free = 0; + ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); + ssize_t ret = -1; + + if ((size_t)buf_free >= sizeof(_header) + buflen) { + ret = ::write(_fd, _header.bytes, sizeof(_header)); + + if (ret == sizeof(_header)) { + ret = ::write(_fd, buffer, buflen); } - - break; } + unlock(Write); + return ret; } class RtpsDev : public DevCommon { public: - RtpsDev(ReadBuffer *_read_buffer); + RtpsDev(ReadBuffer *read_buffer); virtual ~RtpsDev() {} virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); protected: - ReadBuffer *_read_buffer; - - static const uint8_t HEADER_SIZE = 9; + static const uint8_t HEADER_SIZE = 10; }; RtpsDev::RtpsDev(ReadBuffer *read_buffer) - : DevCommon("/dev/rtps") - , _read_buffer{read_buffer} + : DevCommon("/dev/rtps", read_buffer) { - memcpy(_header, Sp2HeaderMagic, 3); - _header[3] = MessageType::Rtps; - memset(&_header[4], 0, 4); - + _header.fields.magic = Sp2HeaderMagic; + _header.fields.len_h = 0; + _header.fields.len_l = 0; + _header.fields.checksum = 0; + _header.fields.type = static_cast(MessageType::Rtps); } ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) { - int i, ret; - uint16_t packet_len, payload_len; - - if (!_had_data) { - return 0; - } - lock(Read); - ret = _read_buffer->read(_fd); - if (ret < 0) { - goto end; - } + scan_for_packets(); + cleanup(); - ret = 0; + // If we have already a packet ready in the current buffer, we don't have to read. + int ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); - if (_read_buffer->buf_size < Sp2HeaderSize) { - goto end; + if (ret > 0) { + unlock(Read); + return ret; } - // Search for a rtps packet on buffer to send it - i = 0; - - while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && - (_read_buffer->buffer[i] != 'S' - || _read_buffer->buffer[i + 1] != 'P' - || _read_buffer->buffer[i + 2] != '2' - || _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Rtps)) { - i++; - } - - // We need at least the first six bytes to get packet len - if ((unsigned)i >= _read_buffer->buf_size - Sp2HeaderSize) { - goto end; - } - - payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5]; - packet_len = payload_len + Sp2HeaderSize; + // Otherwise, we have to do a read. + ret = _read_buffer->read(_fd); - // packet is bigger than what we've read, better luck next time - if ((unsigned)i + packet_len > _read_buffer->buf_size) { - goto end; + if (ret <= 0) { + unlock(Read); + return ret; } - // buffer should be big enough to hold a rtps packet - if (packet_len > buflen) { - ret = -EMSGSIZE; - goto end; - } + scan_for_packets(); - _read_buffer->copy(buffer, i + Sp2HeaderSize, payload_len); - _read_buffer->remove(i, packet_len); - ret = payload_len; + // And check again. + ret = try_to_copy_data(buffer, buflen, MessageType::Rtps); -end: unlock(Read); return ret; } ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) { - /* - * we need to look into the data to make sure the output is locked for the duration - * of a whole packet. - * assumptions: - * - packet header is written all at once (or at least it contains the payload length) - * - a single write call does not contain multiple (or parts of multiple) packets - */ - ssize_t ret = 0; - uint16_t payload_len; - - switch (_parser_state) { - case ParserState::Idle: - ASSERT(buflen >= HEADER_SIZE); - - if (memcmp(buffer, ">>>", 3) != 0) { - PX4_ERR("parser error"); - return 0; + _header.fields.len_h = (buflen >> 8) & 0x7f; + _header.fields.len_l = buflen & 0xff; + _header.fields.checksum = _header.bytes[0] ^ _header.bytes[1] ^ _header.bytes[2]; + lock(Write); + int buf_free = 0; + ssize_t ret = -1; + ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); + + if ((size_t)buf_free >= sizeof(_header) + buflen) { + ret = ::write(_fd, _header.bytes, sizeof(_header)); + + if (ret == sizeof(_header)) { + ret = ::write(_fd, buffer, buflen); } - - payload_len = ((uint16_t)buffer[5] << 8) | buffer[6]; - _packet_len = payload_len + HEADER_SIZE; - _parser_state = ParserState::GotLength; - lock(Write); - - /* FALLTHROUGH */ - - case ParserState::GotLength: { - _packet_len -= buflen; - int buf_free; - ::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); - - // TODO should I care about this for rtps? - if ((unsigned)buf_free < buflen) { - //let write fail, to let rtps know the buffer would overflow - //(this is because in the ioctl we pretend there is always enough space) - ret = -1; - - } else { - _header[4] = (uint8_t)((buflen >> 8) & 0xff); - _header[5] = (uint8_t)(buflen & 0xff); - ::write(_fd, _header, 8); - ret = ::write(_fd, buffer, buflen); - } - - if (_packet_len == 0) { - unlock(Write); - _parser_state = ParserState::Idle; - } - } - - break; } + unlock(Write); + return ret; } @@ -590,7 +749,7 @@ int protocol_splitter_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) { if (objects) { - PX4_ERR("already running"); + PX4_WARN("already running"); return 1; } @@ -605,6 +764,15 @@ int protocol_splitter_main(int argc, char *argv[]) return -1; } + bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes received"); + bytes_lost_count = perf_alloc(PC_COUNT, "protocol_splitter: bytes unused/lost"); + header_bytes_received_count = perf_alloc(PC_COUNT, "protocol_splitter: header bytes received"); + mavlink_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs parsed"); + mavlink_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: MAVLink msgs bytes parsed"); + rtps_messages_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs parsed"); + rtps_bytes_parsed_count = perf_alloc(PC_COUNT, "protocol_splitter: RTPS msgs bytes parsed"); + buffer_drops = perf_alloc(PC_COUNT, "protocol_splitter: buffer drops"); + strncpy(objects->device_name, argv[2], sizeof(objects->device_name)); sem_init(&objects->r_lock, 1, 1); sem_init(&objects->w_lock, 1, 1); @@ -638,6 +806,15 @@ int protocol_splitter_main(int argc, char *argv[]) sem_destroy(&objects->w_lock); delete objects; objects = nullptr; + + perf_free(bytes_received_count); + perf_free(header_bytes_received_count); + perf_free(bytes_lost_count); + perf_free(mavlink_messages_parsed_count); + perf_free(mavlink_bytes_parsed_count); + perf_free(rtps_messages_parsed_count); + perf_free(rtps_bytes_parsed_count); + perf_free(buffer_drops); } } @@ -648,6 +825,13 @@ int protocol_splitter_main(int argc, char *argv[]) if (objects) { PX4_INFO("running"); + if (sem_wait(&objects->r_lock) != 0) { + return -1; + } + + objects->read_buffer->print_stats(); + sem_post(&objects->r_lock); + } else { PX4_INFO("not running"); } diff --git a/src/drivers/pwm_input/Kconfig b/src/drivers/pwm_input/Kconfig new file mode 100644 index 000000000000..85481a2048d5 --- /dev/null +++ b/src/drivers/pwm_input/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PWM_INPUT + bool "pwm_input" + default n + ---help--- + Enable support for pwm_input \ No newline at end of file diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index bdb798c29184..b5df8f3cc6f7 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "pwm_input.h" +#include int PWMIN::task_spawn(int argc, char *argv[]) @@ -65,6 +66,25 @@ PWMIN::start() void PWMIN::timer_init(void) { + /* TODO + * - use gpio+irq directly instead of timer (if accurate enough) + * - make pin configurable + */ + + /* reserve the pin + timer */ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + if ((GPIO_PWM_IN & (GPIO_PORT_MASK | GPIO_PIN_MASK)) == + (timer_io_channels[i].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK))) { + int ret1 = io_timer_allocate_channel(i, IOTimerChanMode_PWMIn); + int ret2 = io_timer_allocate_timer(timer_io_channels[i].timer_index, IOTimerChanMode_PWMIn); + + if (ret1 != 0 || ret2 != 0) { + PX4_ERR("timer/channel alloc failed (%i %i)", ret1, ret2); + return; + } + } + } + /* run with interrupts disabled in case the timer is already * setup. We don't want it firing while we are doing the setup */ irqstate_t flags = px4_enter_critical_section(); @@ -159,13 +179,14 @@ PWMIN::publish(uint16_t status, uint32_t period, uint32_t pulse_width) _pulses_captured++; } -void -PWMIN::print_info(void) +int +PWMIN::print_status() { - PX4_INFO("count=%u period=%u width=%u\n", + PX4_INFO("count=%u period=%u width=%u", static_cast(_pulses_captured), static_cast(_last_period), static_cast(_last_width)); + return 0; } int @@ -184,7 +205,6 @@ Measures the PWM input on AUX5 (or MAIN5) via a timer capture ISR and publishes PRINT_MODULE_USAGE_NAME("pwm_input", "system"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_COMMAND_DESCR("test", "prints PWM capture info."); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return PX4_OK; @@ -193,21 +213,7 @@ Measures the PWM input on AUX5 (or MAIN5) via a timer capture ISR and publishes int PWMIN::custom_command(int argc, char *argv[]) { - const char *input = argv[0]; - auto *obj = get_instance(); - - if (!is_running() || !obj) { - PX4_ERR("not running"); - return PX4_ERROR; - } - - if (!strcmp(input, "test")) { - obj->print_info(); - return PX4_OK; - } - - print_usage(); - return PX4_ERROR; + return print_usage(); } extern "C" __EXPORT int pwm_input_main(int argc, char *argv[]) diff --git a/src/drivers/pwm_input/pwm_input.h b/src/drivers/pwm_input/pwm_input.h index 469b2ea8d5a1..71e8f6269af5 100644 --- a/src/drivers/pwm_input/pwm_input.h +++ b/src/drivers/pwm_input/pwm_input.h @@ -125,7 +125,7 @@ class PWMIN : public ModuleBase public: void start(); void publish(uint16_t status, uint32_t period, uint32_t pulse_width); - void print_info(void); + int print_status() override; static int pwmin_tim_isr(int irq, void *context, void *arg); diff --git a/src/drivers/pwm_out/CMakeLists.txt b/src/drivers/pwm_out/CMakeLists.txt index 017865fdb4c5..1e5fa884bcc7 100644 --- a/src/drivers/pwm_out/CMakeLists.txt +++ b/src/drivers/pwm_out/CMakeLists.txt @@ -30,16 +30,25 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + px4_add_module( MODULE drivers__pwm_out MAIN pwm_out COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" SRCS PWMOut.cpp PWMOut.hpp + MODULE_CONFIG + module.yaml DEPENDS arch_io_pins mixer mixer_module - output_limit ) diff --git a/src/drivers/pwm_out/Kconfig b/src/drivers/pwm_out/Kconfig new file mode 100644 index 000000000000..a263be9a3e05 --- /dev/null +++ b/src/drivers/pwm_out/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PWM_OUT + bool "pwm_out" + default n + ---help--- + Enable support for pwm_out \ No newline at end of file diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 07ec2a44e1d4..27d1a9ed7055 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * 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 @@ -33,9 +33,11 @@ #include "PWMOut.hpp" +#include + pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER; static px4::atomic _objects[PWM_OUT_MAX_INSTANCES] {}; -static bool _pwm_out_started = false; +static px4::atomic_bool _require_arming[PWM_OUT_MAX_INSTANCES] {}; static bool is_running() { @@ -56,14 +58,16 @@ PWMOut::PWMOut(int instance, uint8_t output_base) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval")) { - _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); - _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + if (!_mixing_output.useDynamicMixing()) { + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + } } PWMOut::~PWMOut() { /* make sure servos are off */ - up_pwm_servo_deinit(); // TODO: review for multi + up_pwm_servo_deinit(_pwm_mask); /* clean up the alternate device node */ unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); @@ -81,8 +85,6 @@ int PWMOut::init() return ret; } - // XXX best would be to register / de-register the device depending on modes - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); @@ -94,8 +96,15 @@ int PWMOut::init() _mixing_output.setDriverInstance(_class_instance); - /* force a reset of the update rate */ - _current_update_rate = 0; + if (_mixing_output.useDynamicMixing()) { + _num_outputs = FMU_MAX_ACTUATORS; + + } else { + _num_outputs = math::min(FMU_MAX_ACTUATORS - (int)_output_base, MAX_PER_INSTANCE); + } + + _pwm_mask = ((1u << _num_outputs) - 1) << _output_base; + _mixing_output.setMaxNumOutputs(_num_outputs); // Getting initial parameter values update_params(); @@ -105,242 +114,6 @@ int PWMOut::init() return 0; } -int PWMOut::set_mode(Mode mode) -{ - unsigned old_mask = _pwm_mask; - - /* - * Configure for PWM output. - * - * Note that regardless of the configured mode, the task is always - * listening and mixing; the mode just selects which of the channels - * are presented on the output pins. - */ - switch (mode) { - case MODE_1PWM: - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0001 << _output_base; - _pwm_initialized = false; - _num_outputs = 1; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM - up_input_capture_set(2, Rising, 0, NULL, NULL); - up_input_capture_set(3, Rising, 0, NULL, NULL); - PX4_DEBUG("MODE_2PWM2CAP"); -#endif - - /* FALLTHROUGH */ - - case MODE_2PWM: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_2PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0011 << _output_base; - _pwm_initialized = false; - _num_outputs = 2; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_3PWM1CAP"); - up_input_capture_set(3, Rising, 0, NULL, NULL); -#endif - - /* FALLTHROUGH */ - - case MODE_3PWM: // v1 multi-port with flow control lines as PWM - PX4_DEBUG("MODE_3PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'0111 << _output_base; - _pwm_initialized = false; - _num_outputs = 3; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_4PWM1CAP: - PX4_DEBUG("MODE_4PWM1CAP"); - up_input_capture_set(4, Rising, 0, NULL, NULL); -#endif - - /* FALLTHROUGH */ - - case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs - PX4_DEBUG("MODE_4PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 4; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_4PWM2CAP: - PX4_DEBUG("MODE_4PWM2CAP"); - up_input_capture_set(5, Rising, 0, NULL, NULL); - - /* default output rates */ - _pwm_default_rate = 400; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0000'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 4; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_CAPTURE) - - case MODE_5PWM1CAP: - PX4_DEBUG("MODE_5PWM1CAP"); - up_input_capture_set(5, Rising, 0, NULL, NULL); -#endif - - /* FALLTHROUGH */ - - case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs - PX4_DEBUG("MODE_5PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0001'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 5; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case MODE_6PWM: - PX4_DEBUG("MODE_6PWM"); - - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'0011'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 6; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case MODE_8PWM: - PX4_DEBUG("MODE_8PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'0000'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 8; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case MODE_12PWM: - PX4_DEBUG("MODE_12PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0000'1111'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 12; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case MODE_14PWM: - PX4_DEBUG("MODE_14PWM"); - /* default output rates */ - _pwm_default_rate = 50; - _pwm_alt_rate = 50; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0b0011'1111'1111'1111 << _output_base; - _pwm_initialized = false; - _num_outputs = 14; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - break; -#endif - - case MODE_NONE: - PX4_DEBUG("MODE_NONE"); - - _pwm_default_rate = 10; /* artificially reduced output rate */ - _pwm_alt_rate = 10; - _pwm_alt_rate_channels = 0; - _pwm_mask = 0x0; - _pwm_initialized = false; - _num_outputs = 0; - _mixing_output.setMaxNumOutputs(_num_outputs); - update_params(); - - if (old_mask != _pwm_mask) { - /* disable servo outputs - no need to set rates */ - up_pwm_servo_deinit(); // TODO: review for multi - } - - break; - - default: - return -EINVAL; - } - - _mode = mode; - return OK; -} - /* When set_pwm_rate is called from either of the 2 IOCTLs: * * PWM_SERVO_SET_UPDATE_RATE - Sets the "alternate" channel's rate to the callers's rate specified @@ -367,8 +140,12 @@ int PWMOut::set_mode(Mode mode) * For Oneshot there is no rate, 0 is therefore used * to select Oneshot mode */ -int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) +int PWMOut::set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate) { + if (_mixing_output.useDynamicMixing()) { + return -EINVAL; + } + PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate); for (unsigned pass = 0; pass < 2; pass++) { @@ -383,14 +160,14 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_ * common settings and can not be independent in terms of count frequency * (granularity of pulse width) and rate (period of repetition). * - * To say it another way, all channels in a group moust have the same + * To say it another way, all channels in a group must have the same * rate and mode. (See rates above.) */ for (unsigned group = 0; group < FMU_MAX_ACTUATORS; group++) { // get the channel mask for this rate group - uint32_t mask = up_pwm_servo_get_rate_group(group); + uint32_t mask = _pwm_mask & up_pwm_servo_get_rate_group(group); if (mask == 0) { continue; @@ -402,7 +179,7 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_ if (pass == 0) { // preflight if ((alt != 0) && (alt != mask)) { - PX4_WARN("rate group %u mask %x bad overlap %x", group, mask, alt); + PX4_WARN("rate group %u mask %" PRIx32 " bad overlap %" PRIx32, group, mask, alt); // not a legal map, bail return -EINVAL; } @@ -447,11 +224,6 @@ int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_ return OK; } -int PWMOut::set_i2c_bus_clock(unsigned bus, unsigned clock_hz) -{ - return device::I2C::set_bus_clock(bus, clock_hz); -} - void PWMOut::update_current_rate() { /* @@ -475,7 +247,9 @@ void PWMOut::update_current_rate() // max interval 0.5 - 100 ms (10 - 2000Hz) const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000); - PX4_INFO("instance: %d, MAX RATE: %d, default: %d, alt: %d", _instance, max_rate, _pwm_default_rate, _pwm_alt_rate); + if (_current_update_rate != max_rate) { + PX4_INFO("instance: %d, max rate: %d, default: %d, alt: %d", _instance, max_rate, _pwm_default_rate, _pwm_alt_rate); + } _current_update_rate = max_rate; _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); @@ -486,7 +260,7 @@ int PWMOut::task_spawn(int argc, char *argv[]) for (unsigned instance = 0; instance < (sizeof(_objects) / sizeof(_objects[0])); instance++) { if (instance < PWM_OUT_MAX_INSTANCES) { - uint8_t base = instance * 8; // TODO: configurable + uint8_t base = instance * MAX_PER_INSTANCE; // TODO: configurable PWMOut *dev = new PWMOut(instance, base); if (dev) { @@ -499,6 +273,11 @@ int PWMOut::task_spawn(int argc, char *argv[]) return PX4_ERROR; } + // only start one instance with dynamic mixing + if (dev->_mixing_output.useDynamicMixing()) { + break; + } + } else { PX4_ERR("alloc failed"); } @@ -511,46 +290,140 @@ int PWMOut::task_spawn(int argc, char *argv[]) } } - _pwm_out_started = true; - return PX4_OK; } -void PWMOut::capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +bool PWMOut::update_pwm_out_state(bool on) { - PWMOut *dev = static_cast(context); - dev->capture_callback(chan_index, edge_time, edge_state, overflow); -} + if (on && !_pwm_initialized && _pwm_mask != 0) { -void PWMOut::capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) -{ - fprintf(stdout, "FMU: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow); -} + if (_mixing_output.useDynamicMixing()) { -void PWMOut::update_pwm_out_state(bool on) -{ - if (on && !_pwm_initialized && _pwm_mask != 0) { - up_pwm_servo_init(_pwm_mask); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); - _pwm_initialized = true; + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + _timer_rates[timer] = -1; + + uint32_t channels = io_timer_get_group(timer); + + if (channels == 0) { + continue; + } + + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + + int32_t tim_config = 0; + param_t handle = param_find(param_name); + param_get(handle, &tim_config); + + if (tim_config > 0) { + _timer_rates[timer] = tim_config; + + } else if (tim_config == -1) { // OneShot + _timer_rates[timer] = 0; + + } else { + _pwm_mask &= ~channels; // don't use for pwm + } + } + + int ret = up_pwm_servo_init(_pwm_mask); + + if (ret < 0) { + PX4_ERR("up_pwm_servo_init failed (%i)", ret); + return false; + } + + _pwm_mask = ret; + + // set the timer rates + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); + + if (channels == 0) { + continue; + } + + ret = up_pwm_servo_set_rate_group_update(timer, _timer_rates[timer]); + + if (ret != 0) { + PX4_ERR("up_pwm_servo_set_rate_group_update failed for timer %i, rate %i (%i)", timer, _timer_rates[timer], ret); + _timer_rates[timer] = -1; + _pwm_mask &= ~channels; + } + } + + _pwm_initialized = true; + + // disable unused functions + for (unsigned i = 0; i < _num_outputs; ++i) { + if (((1 << i) & _pwm_mask) == 0) { + _mixing_output.disableFunction(i); + } + } + + } else { + // Collect all PWM masks from all instances + uint32_t pwm_mask_new = 0; + // Collect the PWM alt rate channels across all instances + uint32_t pwm_alt_rate_channels_new = 0; + + for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + + pwm_mask_new |= _objects[i].load()->get_pwm_mask(); + pwm_alt_rate_channels_new |= _objects[i].load()->get_alt_rate_channels(); + } + } + + // Initialize the PWM output state for all instances + // this is re-done once per instance, but harmless + int ret = up_pwm_servo_init(pwm_mask_new); + + if (ret >= 0) { + for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { + if (_objects[i].load()) { + _objects[i].load()->set_pwm_mask(_objects[i].load()->get_pwm_mask() & ret); + } + } + + // Set rate is not affecting non-masked channels, so can be called + // individually + set_pwm_rate(get_alt_rate_channels(), get_default_rate(), get_alt_rate()); + + _pwm_initialized = true; + + // Other instances need to call up_pwm_servo_arm again after we initialized + for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) { + if (i != _instance) { + _require_arming[i].store(true); + } + } + + } else { + PX4_ERR("up_pwm_servo_init failed (%i)", ret); + } + } } - up_pwm_servo_arm(on); // TODO REVIEW for multi + _require_arming[_instance].store(false); + up_pwm_servo_arm(on, _pwm_mask); + return true; } bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - if (_test_mode) { - return false; - } - /* output to the servos */ if (_pwm_initialized) { - for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) { - up_pwm_servo_set(_output_base + i, outputs[i]); + for (size_t i = 0; i < num_outputs; i++) { + if (!_mixing_output.isFunctionSet(i)) { + // do not run any signal on disabled channels + outputs[i] = 0; + } + + if (_pwm_mask & (1 << (i + _output_base))) { + up_pwm_servo_set(_output_base + i, outputs[i]); + } } } @@ -558,7 +431,7 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], * the oneshots with updated values. */ if (num_control_groups_updated > 0) { - up_pwm_update(); // TODO: review for multi + up_pwm_update(_pwm_mask); } return true; @@ -574,25 +447,27 @@ void PWMOut::Run() return; } + SmartLock lock_guard(_lock); + perf_begin(_cycle_perf); perf_count(_interval_perf); - // push backup schedule - ScheduleDelayed(_backup_schedule_interval_us); - - if (_new_mode_request.load() != _mode) { - set_mode(_new_mode_request.load()); - _new_mode_request.store(_mode); + if (!_mixing_output.useDynamicMixing()) { + // push backup schedule + ScheduleDelayed(_backup_schedule_interval_us); } _mixing_output.update(); /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.armed().in_esc_calibration_mode; + bool pwm_on = _mixing_output.armed().armed || (_num_disarmed_set > 0) || _mixing_output.useDynamicMixing() + || _mixing_output.armed().in_esc_calibration_mode; - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - update_pwm_out_state(pwm_on); + if (_pwm_on != pwm_on || _require_arming[_instance].load()) { + + if (update_pwm_out_state(pwm_on)) { + _pwm_on = pwm_on; + } } // check for parameter updates @@ -602,10 +477,12 @@ void PWMOut::Run() _parameter_update_sub.copy(&pupdate); // update parameters from storage - update_params(); + if (_mixing_output.useDynamicMixing()) { // do not update PWM params for now (was interfering with VTOL PWM settings) + update_params(); + } } - if (_current_update_rate == 0) { + if (_pwm_initialized && _current_update_rate == 0 && !_mixing_output.useDynamicMixing()) { update_current_rate(); } @@ -619,8 +496,7 @@ void PWMOut::update_params() { updateParams(); - // skip update when armed - if (_mixing_output.armed().armed) { + if (_mixing_output.useDynamicMixing()) { return; } @@ -628,6 +504,7 @@ void PWMOut::update_params() int32_t pwm_max_default = PWM_DEFAULT_MAX; int32_t pwm_disarmed_default = 0; int32_t pwm_rate_default = 50; + int32_t pwm_default_channels = 0; const char *prefix; @@ -638,6 +515,7 @@ void PWMOut::update_params() param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default); + param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels); } else if (_class_instance == CLASS_DEVICE_SECONDARY) { prefix = "PWM_AUX"; @@ -646,6 +524,7 @@ void PWMOut::update_params() param_get(param_find("PWM_AUX_MAX"), &pwm_max_default); param_get(param_find("PWM_AUX_DISARM"), &pwm_disarmed_default); param_get(param_find("PWM_AUX_RATE"), &pwm_rate_default); + param_get(param_find("PWM_AUX_OUT"), &pwm_default_channels); } else if (_class_instance == CLASS_DEVICE_TERTIARY) { prefix = "PWM_EXTRA"; @@ -660,6 +539,14 @@ void PWMOut::update_params() return; } + uint32_t single_ch = 0; + uint32_t pwm_default_channel_mask = 0; + + while ((single_ch = pwm_default_channels % 10)) { + pwm_default_channel_mask |= 1 << (single_ch - 1); + pwm_default_channels /= 10; + } + // update the counter // this is needed to decide if disarmed PWM output should be turned on or not int num_disarmed_set = 0; @@ -673,17 +560,20 @@ void PWMOut::update_params() int32_t pwm_min = -1; if (param_get(param_find(str), &pwm_min) == PX4_OK) { - if (pwm_min >= 0) { - _mixing_output.minValue(i) = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN); + if (pwm_min >= 0 && pwm_min != 1000) { + _mixing_output.minValue(i) = math::constrain(pwm_min, (int32_t) PWM_LOWEST_MIN, (int32_t) PWM_HIGHEST_MIN); if (pwm_min != _mixing_output.minValue(i)) { int32_t pwm_min_new = _mixing_output.minValue(i); param_set(param_find(str), &pwm_min_new); } - } else { + } else if (pwm_default_channel_mask & 1 << i) { _mixing_output.minValue(i) = pwm_min_default; } + + } else { + PX4_ERR("param %s not found", str); } } @@ -693,17 +583,20 @@ void PWMOut::update_params() int32_t pwm_max = -1; if (param_get(param_find(str), &pwm_max) == PX4_OK) { - if (pwm_max >= 0) { - _mixing_output.maxValue(i) = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX); + if (pwm_max >= 0 && pwm_max != 2000) { + _mixing_output.maxValue(i) = math::constrain(pwm_max, (int32_t) PWM_LOWEST_MAX, (int32_t) PWM_HIGHEST_MAX); if (pwm_max != _mixing_output.maxValue(i)) { int32_t pwm_max_new = _mixing_output.maxValue(i); param_set(param_find(str), &pwm_max_new); } - } else { + } else if (pwm_default_channel_mask & 1 << i) { _mixing_output.maxValue(i) = pwm_max_default; } + + } else { + PX4_ERR("param %s not found", str); } } @@ -714,13 +607,24 @@ void PWMOut::update_params() if (param_get(param_find(str), &pwm_failsafe) == PX4_OK) { if (pwm_failsafe >= 0) { - _mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, 0, PWM_HIGHEST_MAX); + _mixing_output.failsafeValue(i) = math::constrain(pwm_failsafe, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX); if (pwm_failsafe != _mixing_output.failsafeValue(i)) { int32_t pwm_fail_new = _mixing_output.failsafeValue(i); param_set(param_find(str), &pwm_fail_new); } + + } else { + if (pwm_default_channel_mask & 1 << i) { + _mixing_output.failsafeValue(i) = PWM_MOTOR_OFF; + + } else { + _mixing_output.failsafeValue(i) = PWM_SERVO_STOP; + } } + + } else { + PX4_ERR("param %s not found", str); } } @@ -730,17 +634,20 @@ void PWMOut::update_params() int32_t pwm_dis = -1; if (param_get(param_find(str), &pwm_dis) == PX4_OK) { - if (pwm_dis >= 0) { - _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX); + if (pwm_dis >= 0 && pwm_dis != 900) { + _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, (int32_t) 0, (int32_t) PWM_HIGHEST_MAX); if (pwm_dis != _mixing_output.disarmedValue(i)) { int32_t pwm_dis_new = _mixing_output.disarmedValue(i); param_set(param_find(str), &pwm_dis_new); } - } else { + } else if (pwm_default_channel_mask & 1 << i) { _mixing_output.disarmedValue(i) = pwm_disarmed_default; } + + } else { + PX4_ERR("param %s not found", str); } if (_mixing_output.disarmedValue(i) > 0) { @@ -762,6 +669,9 @@ void PWMOut::update_params() } else { reverse_pwm_mask = reverse_pwm_mask & ~(1 << i); } + + } else { + PX4_ERR("param %s not found", str); } } } @@ -773,7 +683,11 @@ void PWMOut::update_params() sprintf(str, "%s_TRIM%u", prefix, i + 1); float pval = 0.0f; - param_get(param_find(str), &pval); + + if (param_get(param_find(str), &pval) != PX4_OK) { + PX4_ERR("param %s not found", str); + } + values[i] = roundf(10000 * pval); } @@ -784,48 +698,11 @@ void PWMOut::update_params() _num_disarmed_set = num_disarmed_set; } -int PWMOut::ioctl(file *filp, int cmd, unsigned long arg) +int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret; - - /* try it as a Capture ioctl next */ - ret = capture_ioctl(filp, cmd, arg); - - if (ret != -ENOTTY) { - return ret; - } - - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch (_mode) { - case MODE_1PWM: - case MODE_2PWM: - case MODE_3PWM: - case MODE_4PWM: - case MODE_5PWM: - case MODE_2PWM2CAP: - case MODE_3PWM1CAP: - case MODE_4PWM1CAP: - case MODE_4PWM2CAP: - case MODE_5PWM1CAP: -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - case MODE_6PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - case MODE_8PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - case MODE_12PWM: -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - case MODE_14PWM: -#endif - ret = pwm_ioctl(filp, cmd, arg); - break; + SmartLock lock_guard(_lock); - default: - PX4_DEBUG("pwm_out%u, not in a PWM mode", _instance); - break; - } + int ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) { @@ -835,14 +712,12 @@ int PWMOut::ioctl(file *filp, int cmd, unsigned long arg) return ret; } -int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) +int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) { int ret = OK; PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg); - lock(); - switch (cmd) { case PWM_SERVO_ARM: update_pwm_out_state(true); @@ -850,8 +725,6 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: - case PWM_SERVO_SET_FORCE_SAFETY_OFF: - case PWM_SERVO_SET_FORCE_SAFETY_ON: break; case PWM_SERVO_DISARM: @@ -883,56 +756,33 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) *(uint32_t *)arg = _pwm_alt_rate_channels; break; - case PWM_SERVO_SET_FAILSAFE_PWM: { + case PWM_SERVO_GET_FAILSAFE_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { - ret = -EINVAL; - break; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _mixing_output.failsafeValue(i) = PWM_HIGHEST_MAX; - - } - -#if PWM_LOWEST_MIN > 0 - - else if (pwm->values[i] < PWM_LOWEST_MIN) { - _mixing_output.failsafeValue(i) = PWM_LOWEST_MIN; - - } - -#endif - - else { - _mixing_output.failsafeValue(i) = pwm->values[i]; - } + for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.failsafeValue(i); } + pwm->channel_count = FMU_MAX_ACTUATORS; break; } - case PWM_SERVO_GET_FAILSAFE_PWM: { + case PWM_SERVO_GET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.failsafeValue(i); + pwm->values[i] = _mixing_output.disarmedValue(i); } pwm->channel_count = FMU_MAX_ACTUATORS; break; } - case PWM_SERVO_SET_DISARMED_PWM: { + case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -940,54 +790,44 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _mixing_output.disarmedValue(i) = PWM_HIGHEST_MAX; + } else if (pwm->values[i] > PWM_HIGHEST_MIN) { + _mixing_output.minValue(i) = PWM_HIGHEST_MIN; + } #if PWM_LOWEST_MIN > 0 else if (pwm->values[i] < PWM_LOWEST_MIN) { - _mixing_output.disarmedValue(i) = PWM_LOWEST_MIN; + _mixing_output.minValue(i) = PWM_LOWEST_MIN; } #endif else { - _mixing_output.disarmedValue(i) = pwm->values[i]; - } - } - - /* - * update the counter - * this is needed to decide if disarmed PWM output should be turned on or not - */ - _num_disarmed_set = 0; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - if (_mixing_output.disarmedValue(i) > 0) { - _num_disarmed_set++; + _mixing_output.minValue(i) = pwm->values[i]; } } break; } - case PWM_SERVO_GET_DISARMED_PWM: { + case PWM_SERVO_GET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.disarmedValue(i); + pwm->values[i] = _mixing_output.minValue(i); } pwm->channel_count = FMU_MAX_ACTUATORS; + arg = (unsigned long)&pwm; break; } - case PWM_SERVO_SET_MIN_PWM: { + case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { + if (pwm->channel_count > FMU_MAX_ACTUATORS || _mixing_output.useDynamicMixing()) { ret = -EINVAL; break; } @@ -995,1193 +835,158 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg) for (unsigned i = 0; i < pwm->channel_count; i++) { if (pwm->values[i] == 0) { /* ignore 0 */ - } else if (pwm->values[i] > PWM_HIGHEST_MIN) { - _mixing_output.minValue(i) = PWM_HIGHEST_MIN; + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _mixing_output.maxValue(i) = PWM_LOWEST_MAX; + + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _mixing_output.maxValue(i) = PWM_HIGHEST_MAX; + } else { + _mixing_output.maxValue(i) = pwm->values[i]; } + } -#if PWM_LOWEST_MIN > 0 - - else if (pwm->values[i] < PWM_LOWEST_MIN) { - _mixing_output.minValue(i) = PWM_LOWEST_MIN; - } - -#endif - - else { - _mixing_output.minValue(i) = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_GET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.minValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - arg = (unsigned long)&pwm; - break; - } - - case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - /* discard if too many values are sent */ - if (pwm->channel_count > FMU_MAX_ACTUATORS) { - ret = -EINVAL; - break; - } - - for (unsigned i = 0; i < pwm->channel_count; i++) { - if (pwm->values[i] == 0) { - /* ignore 0 */ - } else if (pwm->values[i] < PWM_LOWEST_MAX) { - _mixing_output.maxValue(i) = PWM_LOWEST_MAX; - - } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _mixing_output.maxValue(i) = PWM_HIGHEST_MAX; - - } else { - _mixing_output.maxValue(i) = pwm->values[i]; - } - } - - break; - } - - case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { - pwm->values[i] = _mixing_output.maxValue(i); - } - - pwm->channel_count = FMU_MAX_ACTUATORS; - arg = (unsigned long)&pwm; - } - break; - - case PWM_SERVO_GET_TRIM_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (_mixing_output.mixers() == nullptr) { - memset(pwm, 0, sizeof(pwm_output_values)); - PX4_WARN("warning: trim values not valid - no mixer loaded"); - - } else { - - pwm->channel_count = _mixing_output.mixers()->get_trims((int16_t *)pwm->values); - } - } - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case PWM_SERVO_SET(13): - case PWM_SERVO_SET(12): - case PWM_SERVO_SET(11): - case PWM_SERVO_SET(10): - case PWM_SERVO_SET(9): - case PWM_SERVO_SET(8): - if (_mode < MODE_14PWM) { - ret = -EINVAL; - break; - } - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - /* FALLTHROUGH */ - case PWM_SERVO_SET(7): - - /* FALLTHROUGH */ - case PWM_SERVO_SET(6): - if (_mode < MODE_8PWM) { - ret = -EINVAL; - break; - } - -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - /* FALLTHROUGH */ - case PWM_SERVO_SET(5): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - /* FALLTHROUGH */ - case PWM_SERVO_SET(4): - if (_mode < MODE_5PWM) { - ret = -EINVAL; - break; - } - -#endif - - /* FALLTHROUGH */ - case PWM_SERVO_SET(3): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(2): - if (_mode < MODE_3PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_SET(1): - case PWM_SERVO_SET(0): - if (arg <= 2100) { - up_pwm_servo_set(cmd - PWM_SERVO_SET(0 + _output_base), arg); - - } else { - ret = -EINVAL; - } - - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case PWM_SERVO_GET(13): - case PWM_SERVO_GET(12): - case PWM_SERVO_GET(11): - case PWM_SERVO_GET(10): - case PWM_SERVO_GET(9): - case PWM_SERVO_GET(8): - if (_mode < MODE_14PWM) { - ret = -EINVAL; - break; - } - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - /* FALLTHROUGH */ - case PWM_SERVO_GET(7): - case PWM_SERVO_GET(6): - if (_mode < MODE_8PWM) { - ret = -EINVAL; - break; - } - -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - /* FALLTHROUGH */ - case PWM_SERVO_GET(5): - if (_mode < MODE_6PWM) { - ret = -EINVAL; - break; - } - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - /* FALLTHROUGH */ - case PWM_SERVO_GET(4): - if (_mode < MODE_5PWM) { - ret = -EINVAL; - break; - } - -#endif - - /* FALLTHROUGH */ - case PWM_SERVO_GET(3): - if (_mode < MODE_4PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(2): - if (_mode < MODE_3PWM) { - ret = -EINVAL; - break; - } - - /* FALLTHROUGH */ - case PWM_SERVO_GET(1): - case PWM_SERVO_GET(0): - *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0 + _output_base)); - break; - - case PWM_SERVO_GET_RATEGROUP(0): - case PWM_SERVO_GET_RATEGROUP(1): - case PWM_SERVO_GET_RATEGROUP(2): - case PWM_SERVO_GET_RATEGROUP(3): -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - case PWM_SERVO_GET_RATEGROUP(4): -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - case PWM_SERVO_GET_RATEGROUP(5): -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - case PWM_SERVO_GET_RATEGROUP(6): - case PWM_SERVO_GET_RATEGROUP(7): -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - case PWM_SERVO_GET_RATEGROUP(8): - case PWM_SERVO_GET_RATEGROUP(9): - case PWM_SERVO_GET_RATEGROUP(10): - case PWM_SERVO_GET_RATEGROUP(11): - case PWM_SERVO_GET_RATEGROUP(12): - case PWM_SERVO_GET_RATEGROUP(13): -#endif - *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0 + _output_base)); - break; - - case PWM_SERVO_GET_COUNT: - switch (_mode) { - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case MODE_14PWM: - *(unsigned *)arg = 14; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case MODE_12PWM: - *(unsigned *)arg = 12; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case MODE_8PWM: - *(unsigned *)arg = 8; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case MODE_6PWM: - *(unsigned *)arg = 6; - break; -#endif - - case MODE_5PWM: - case MODE_5PWM1CAP: - *(unsigned *)arg = 5; - break; - - case MODE_4PWM: - case MODE_4PWM1CAP: - case MODE_4PWM2CAP: - *(unsigned *)arg = 4; - break; - - case MODE_3PWM: - case MODE_3PWM1CAP: - *(unsigned *)arg = 3; - break; - - case MODE_2PWM: - case MODE_2PWM2CAP: - *(unsigned *)arg = 2; - break; - - case MODE_1PWM: - *(unsigned *)arg = 1; - break; - - default: - ret = -EINVAL; - break; - } - - break; - - case PWM_SERVO_SET_MODE: { - switch (arg) { - case PWM_SERVO_MODE_NONE: - ret = set_mode(MODE_NONE); - break; - - case PWM_SERVO_MODE_1PWM: - ret = set_mode(MODE_1PWM); - break; - - case PWM_SERVO_MODE_2PWM: - ret = set_mode(MODE_2PWM); - break; - - case PWM_SERVO_MODE_2PWM2CAP: - ret = set_mode(MODE_2PWM2CAP); - break; - - case PWM_SERVO_MODE_3PWM: - ret = set_mode(MODE_3PWM); - break; - - case PWM_SERVO_MODE_3PWM1CAP: - ret = set_mode(MODE_3PWM1CAP); - break; - - case PWM_SERVO_MODE_4PWM: - ret = set_mode(MODE_4PWM); - break; - - case PWM_SERVO_MODE_4PWM1CAP: - ret = set_mode(MODE_4PWM1CAP); - break; - - case PWM_SERVO_MODE_4PWM2CAP: - ret = set_mode(MODE_4PWM2CAP); - break; - - case PWM_SERVO_MODE_5PWM: - ret = set_mode(MODE_5PWM); - break; - - case PWM_SERVO_MODE_5PWM1CAP: - ret = set_mode(MODE_5PWM1CAP); - break; - - case PWM_SERVO_MODE_6PWM: - ret = set_mode(MODE_6PWM); - break; - - case PWM_SERVO_MODE_8PWM: - ret = set_mode(MODE_8PWM); - break; - - case PWM_SERVO_MODE_12PWM: - ret = set_mode(MODE_12PWM); - break; - - case PWM_SERVO_MODE_14PWM: - ret = set_mode(MODE_14PWM); - break; - - case PWM_SERVO_MODE_4CAP: - ret = set_mode(MODE_4CAP); - break; - - case PWM_SERVO_MODE_5CAP: - ret = set_mode(MODE_5CAP); - break; - - case PWM_SERVO_MODE_6CAP: - ret = set_mode(MODE_6CAP); - break; - - case PWM_SERVO_ENTER_TEST_MODE: - _test_mode = true; - break; - - case PWM_SERVO_EXIT_TEST_MODE: - _test_mode = false; - break; - - default: - ret = -EINVAL; - } - - break; - } - - case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); - - break; - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); - update_params(); - - break; - } - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -void PWMOut::sensor_reset(int ms) -{ - if (ms < 1) { - ms = 1; - } - - board_spi_reset(ms, 0xffff); -} - -void PWMOut::peripheral_reset(int ms) -{ - if (ms < 1) { - ms = 10; - } - - board_peripheral_reset(ms); -} - -int PWMOut::capture_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = -EINVAL; - -#if defined(BOARD_HAS_CAPTURE) - - lock(); - - input_capture_config_t *pconfig = 0; - - input_capture_stats_t *stats = (input_capture_stats_t *)arg; - - if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP || - _mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP || - _mode == MODE_4PWM2CAP) { - - pconfig = (input_capture_config_t *)arg; - } - - switch (cmd) { - - case INPUT_CAP_SET: - if (pconfig) { - ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter, - pconfig->callback, pconfig->context); - } - - break; - - case INPUT_CAP_SET_CALLBACK: - if (pconfig) { - ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context); - } - - break; - - case INPUT_CAP_GET_CALLBACK: - if (pconfig) { - ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context); - } - - break; - - case INPUT_CAP_GET_STATS: - if (arg) { - ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false); - } - - break; - - case INPUT_CAP_GET_CLR_STATS: - if (arg) { - ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true); - } - - break; - - case INPUT_CAP_SET_EDGE: - if (pconfig) { - ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge); - } - - break; - - case INPUT_CAP_GET_EDGE: - if (pconfig) { - ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge); - } - - break; - - case INPUT_CAP_SET_FILTER: - if (pconfig) { - ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter); - } - - break; - - case INPUT_CAP_GET_FILTER: - if (pconfig) { - ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter); - } - - break; - - case INPUT_CAP_GET_COUNT: - ret = OK; - - switch (_mode) { - case MODE_5PWM1CAP: - case MODE_4PWM1CAP: - case MODE_3PWM1CAP: - *(unsigned *)arg = 1; - break; - - case MODE_2PWM2CAP: - case MODE_4PWM2CAP: - *(unsigned *)arg = 2; - break; - - default: - ret = -EINVAL; - break; - } - - break; - - case INPUT_CAP_SET_COUNT: - ret = OK; - - switch (_mode) { - case MODE_3PWM1CAP: - set_mode(MODE_3PWM1CAP); - break; - - case MODE_2PWM2CAP: - set_mode(MODE_2PWM2CAP); - break; - - case MODE_4PWM1CAP: - set_mode(MODE_4PWM1CAP); - break; - - case MODE_4PWM2CAP: - set_mode(MODE_4PWM2CAP); - break; - - case MODE_5PWM1CAP: - set_mode(MODE_5PWM1CAP); - break; - - default: - ret = -EINVAL; - break; - } - - break; - - default: - ret = -ENOTTY; - break; - } - - unlock(); - -#else - ret = -ENOTTY; -#endif - return ret; -} - -int PWMOut::fmu_new_mode(PortMode new_mode) -{ - if (!is_running()) { - return -1; - } - - PWMOut::Mode pwm_mode0 = PWMOut::MODE_NONE; - PWMOut::Mode pwm_mode1 = PWMOut::MODE_NONE; - - switch (new_mode) { - case PORT_FULL_GPIO: - case PORT_MODE_UNSET: - break; - - case PORT_FULL_PWM: - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4 - /* select 4-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_4PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5 - pwm_mode0 = PWMOut::MODE_5PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6 - pwm_mode0 = PWMOut::MODE_6PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8 - pwm_mode0 = PWMOut::MODE_8PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 12 - //pwm_mode0 = PWMOut::MODE_12PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_4PWM; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14 - //pwm_mode0 = PWMOut::MODE_14PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_6PWM; -#endif - break; - - case PORT_PWM1: - /* select 2-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_1PWM; - break; - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - case PORT_PWM14: - /* select 14-pin PWM mode */ - //pwm_mode0 = PWMOut::MODE_14PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_6PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - case PORT_PWM12: - /* select 12-pin PWM mode */ - //pwm_mode0 = PWMOut::MODE_12PWM; - pwm_mode0 = PWMOut::MODE_8PWM; - pwm_mode1 = PWMOut::MODE_4PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - case PORT_PWM8: - /* select 8-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_8PWM; - break; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - case PORT_PWM6: - /* select 6-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_6PWM; - break; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - case PORT_PWM5: - /* select 5-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_5PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM5CAP1: - /* select 5-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_5PWM1CAP; - break; - -# endif -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 4 - - case PORT_PWM4: - /* select 4-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_4PWM; - break; - - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM4CAP1: - /* select 4-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_4PWM1CAP; - break; - - case PORT_PWM4CAP2: - /* select 4-pin PWM mode 2 capture */ - pwm_mode0 = PWMOut::MODE_4PWM2CAP; - break; - -# endif - - case PORT_PWM3: - /* select 3-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_3PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM3CAP1: - /* select 3-pin PWM mode 1 capture */ - pwm_mode0 = PWMOut::MODE_3PWM1CAP; - break; -# endif - - case PORT_PWM2: - /* select 2-pin PWM mode */ - pwm_mode0 = PWMOut::MODE_2PWM; - break; - -# if defined(BOARD_HAS_CAPTURE) - - case PORT_PWM2CAP2: - /* select 2-pin PWM mode 2 capture */ - pwm_mode0 = PWMOut::MODE_2PWM2CAP; - break; - -# endif -#endif - - default: - return -1; - } - - if (PWM_OUT_MAX_INSTANCES > 0) { - PWMOut *pwm0 = _objects[0].load(); // TODO: get_instance(); - - if (pwm0 && pwm_mode0 != pwm0->get_mode()) { - pwm0->request_mode(pwm_mode0); - } - } - - if (PWM_OUT_MAX_INSTANCES > 1) { - PWMOut *pwm1 = _objects[1].load(); // TODO: get_instance(); - - if (pwm1 && pwm_mode1 != pwm1->get_mode()) { - pwm1->request_mode(pwm_mode1); - } - } - - return OK; -} - - -namespace -{ - -int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz) -{ - return PWMOut::set_i2c_bus_clock(bus, clock_hz); -} - -} // namespace - -int PWMOut::test() -{ - int fd; - unsigned servo_count = 0; - unsigned capture_count = 0; - unsigned pwm_value = 1000; - int direction = 1; - int ret; - int rv = -1; - uint32_t rate_limit = 0; - struct input_capture_t { - bool valid; - input_capture_config_t chan; - } capture_conf[INPUT_CAPTURE_MAX_CHANNELS]; - - fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); - - if (fd < 0) { - PX4_ERR("open fail"); - return -1; - } - - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_ENTER_TEST_MODE) < 0) { - PX4_ERR("Failed to Enter pwm test mode"); - goto err_out_no_test; - } - - if (::ioctl(fd, PWM_SERVO_ARM, 0) < 0) { - PX4_ERR("servo arm failed"); - goto err_out; - } - - if (::ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { - PX4_ERR("Unable to get servo count"); - goto err_out; - } - - if (::ioctl(fd, INPUT_CAP_GET_COUNT, (unsigned long)&capture_count) != 0) { - PX4_INFO("Not in a capture mode"); - } - - PX4_INFO("Testing %u servos and %u input captures", (unsigned)servo_count, capture_count); - memset(capture_conf, 0, sizeof(capture_conf)); - - if (capture_count != 0) { - for (unsigned i = 0; i < capture_count; i++) { - // Map to channel number - capture_conf[i].chan.channel = i + servo_count; - - /* Save handler */ - if (::ioctl(fd, INPUT_CAP_GET_CALLBACK, (unsigned long)&capture_conf[i].chan.channel) != 0) { - PX4_ERR("Unable to get capture callback for chan %u\n", capture_conf[i].chan.channel); - goto err_out; - - } else { - input_capture_config_t conf = capture_conf[i].chan; - conf.callback = &PWMOut::capture_trampoline; - conf.context = _objects[0].load(); // TODO PWMOut::get_instance(); - - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) { - capture_conf[i].valid = true; - - } else { - PX4_ERR("Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel); - goto err_out; - } - } - - } - } - - struct pollfd fds; - - fds.fd = 0; /* stdin */ - - fds.events = POLLIN; - - PX4_INFO("Press CTRL-C or 'c' to abort."); - - for (;;) { - /* sweep all servos between 1000..2000 */ - servo_position_t servos[servo_count]; - - for (unsigned i = 0; i < servo_count; i++) { - servos[i] = pwm_value; - } - - for (unsigned i = 0; i < servo_count; i++) { - if (::ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { - PX4_ERR("servo %u set failed", i); - goto err_out; - } + break; } - if (direction > 0) { - if (pwm_value < 2000) { - pwm_value++; + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - } else { - direction = -1; + for (unsigned i = 0; i < FMU_MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.maxValue(i); } - } else { - if (pwm_value > 1000) { - pwm_value--; - - } else { - direction = 1; - } + pwm->channel_count = FMU_MAX_ACTUATORS; + arg = (unsigned long)&pwm; } + break; - /* readback servo values */ - for (unsigned i = 0; i < servo_count; i++) { - servo_position_t value; - - if (::ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) { - PX4_ERR("error reading PWM servo %d", i); - goto err_out; - } +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14 - if (value != servos[i]) { - PX4_ERR("servo %d readback error, got %u expected %u", i, value, servos[i]); - goto err_out; - } + case PWM_SERVO_GET(13): + case PWM_SERVO_GET(12): + case PWM_SERVO_GET(11): + case PWM_SERVO_GET(10): + case PWM_SERVO_GET(9): + case PWM_SERVO_GET(8): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8 + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6 + case PWM_SERVO_GET(5): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5 + case PWM_SERVO_GET(4): +#endif + case PWM_SERVO_GET(3): + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): + if (cmd - PWM_SERVO_GET(0) >= (int)_num_outputs) { + ret = -EINVAL; + break; } - if (capture_count != 0 && (++rate_limit % 500 == 0)) { - for (unsigned i = 0; i < capture_count; i++) { - if (capture_conf[i].valid) { - input_capture_stats_t stats; - stats.chan_in_edges_out = capture_conf[i].chan.channel; - - if (::ioctl(fd, INPUT_CAP_GET_STATS, (unsigned long)&stats) != 0) { - PX4_ERR("Unable to get stats for chan %u\n", capture_conf[i].chan.channel); - goto err_out; - - } else { - fprintf(stdout, "FMU: Status chan:%u edges: %d last time:%lld last state:%d overflows:%d lantency:%u\n", - capture_conf[i].chan.channel, - stats.chan_in_edges_out, - stats.last_time, - stats.last_edge, - stats.overflows, - stats.latnecy); - } - } - } + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0) + _output_base); + break; - } + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 5 + case PWM_SERVO_GET_RATEGROUP(4): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 6 + case PWM_SERVO_GET_RATEGROUP(5): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 8 + case PWM_SERVO_GET_RATEGROUP(6): + case PWM_SERVO_GET_RATEGROUP(7): +#endif +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS >= 14 + case PWM_SERVO_GET_RATEGROUP(8): + case PWM_SERVO_GET_RATEGROUP(9): + case PWM_SERVO_GET_RATEGROUP(10): + case PWM_SERVO_GET_RATEGROUP(11): + case PWM_SERVO_GET_RATEGROUP(12): + case PWM_SERVO_GET_RATEGROUP(13): +#endif + *(uint32_t *)arg = _pwm_mask & up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; - /* Check if user wants to quit */ - char c; - ret = ::poll(&fds, 1, 0); + case PWM_SERVO_GET_COUNT: + *(unsigned *)arg = _num_outputs; + break; - if (ret > 0) { + case MIXERIOCRESET: + _mixing_output.resetMixer(); - ::read(0, &c, 1); + break; - if (c == 0x03 || c == 0x63 || c == 'q') { - PX4_INFO("User abort"); - break; - } - } - } + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixer(buf, buflen); + update_params(); - if (capture_count != 0) { - for (unsigned i = 0; i < capture_count; i++) { - // Map to channel number - if (capture_conf[i].valid) { - /* Save handler */ - if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&capture_conf[i].chan) != 0) { - PX4_ERR("Unable to set capture callback for chan %u\n", capture_conf[i].chan.channel); - goto err_out; - } - } + break; } - } - - rv = 0; -err_out: - - if (::ioctl(fd, PWM_SERVO_SET_MODE, PWM_SERVO_EXIT_TEST_MODE) < 0) { - PX4_ERR("Failed to Exit pwm test mode"); + default: + ret = -ENOTTY; + break; } -err_out_no_test: - ::close(fd); - return rv; + return ret; } int PWMOut::custom_command(int argc, char *argv[]) { - PortMode new_mode = PORT_MODE_UNSET; - const char *verb = argv[0]; - - /* does not operate on a FMU instance */ - if (!strcmp(verb, "i2c")) { - if (argc > 2) { - int bus = strtol(argv[1], 0, 0); - int clock_hz = strtol(argv[2], 0, 0); - int ret = fmu_new_i2c_speed(bus, clock_hz); - - if (ret) { - PX4_ERR("setting I2C clock failed"); - } - - return ret; - } - - return print_usage("not enough arguments"); - } - - if (!strcmp(verb, "sensor_reset")) { - if (argc > 1) { - int reset_time = strtol(argv[1], nullptr, 0); - sensor_reset(reset_time); - - } else { - sensor_reset(0); - PX4_INFO("reset default time"); - } - - return 0; - } - - if (!strcmp(verb, "peripheral_reset")) { - if (argc > 2) { - int reset_time = strtol(argv[2], 0, 0); - peripheral_reset(reset_time); - - } else { - peripheral_reset(0); - PX4_INFO("reset default time"); - } - - return 0; - } - - - /* start pwm_out if not running */ - if (!_pwm_out_started) { - - int ret = PWMOut::task_spawn(argc, argv); - - if (ret) { - return ret; - } - } - - /* - * Mode switches. - */ - if (!strcmp(verb, "mode_gpio")) { - new_mode = PORT_FULL_GPIO; - - } else if (!strcmp(verb, "mode_pwm")) { - new_mode = PORT_FULL_PWM; - - // mode: defines which outputs to drive (others may be used by other tasks such as camera capture) -#if defined(BOARD_HAS_PWM) - - } else if (!strcmp(verb, "mode_pwm1")) { - new_mode = PORT_PWM1; -#endif - -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 - - } else if (!strcmp(verb, "mode_pwm6")) { - new_mode = PORT_PWM6; - -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 5 - - } else if (!strcmp(verb, "mode_pwm5")) { - new_mode = PORT_PWM5; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm5cap1")) { - new_mode = PORT_PWM5CAP1; -# endif - - } else if (!strcmp(verb, "mode_pwm4")) { - new_mode = PORT_PWM4; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm4cap1")) { - new_mode = PORT_PWM4CAP1; - - } else if (!strcmp(verb, "mode_pwm4cap2")) { - new_mode = PORT_PWM4CAP2; -# endif - - } else if (!strcmp(verb, "mode_pwm3")) { - new_mode = PORT_PWM3; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm3cap1")) { - new_mode = PORT_PWM3CAP1; -# endif - - } else if (!strcmp(verb, "mode_pwm2")) { - new_mode = PORT_PWM2; - -# if defined(BOARD_HAS_CAPTURE) - - } else if (!strcmp(verb, "mode_pwm2cap2")) { - new_mode = PORT_PWM2CAP2; -# endif -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 - - } else if (!strcmp(verb, "mode_pwm8")) { - new_mode = PORT_PWM8; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12 - - } else if (!strcmp(verb, "mode_pwm12")) { - new_mode = PORT_PWM12; -#endif -#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 - - } else if (!strcmp(verb, "mode_pwm14")) { - new_mode = PORT_PWM14; -#endif - - } - - /* was a new mode set? */ - if (new_mode != PORT_MODE_UNSET) { - /* switch modes */ - return PWMOut::fmu_new_mode(new_mode); - } - - if (!strcmp(verb, "test")) { - return test(); - } - return print_usage("unknown command"); } int PWMOut::print_status() { if (_class_instance == CLASS_DEVICE_PRIMARY) { - PX4_INFO("%d - PWM_MAIN 0x%04X", _instance, _pwm_mask); + PX4_INFO("%d - PWM_MAIN 0x%04" PRIx32, _instance, _pwm_mask); } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - PX4_INFO("%d - PWM_AUX 0x%04X", _instance, _pwm_mask); + PX4_INFO("%d - PWM_AUX 0x%04" PRIx32, _instance, _pwm_mask); } else if (_class_instance == CLASS_DEVICE_TERTIARY) { - PX4_INFO("%d - PWM_EXTRA 0x%04X", _instance, _pwm_mask); + PX4_INFO("%d - PWM_EXTRA 0x%04" PRIx32, _instance, _pwm_mask); } PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate); - const char *mode_str = nullptr; - - switch (_mode) { - case MODE_NONE: mode_str = "no pwm"; break; - - case MODE_1PWM: mode_str = "pwm1"; break; - - case MODE_2PWM: mode_str = "pwm2"; break; - - case MODE_2PWM2CAP: mode_str = "pwm2cap2"; break; - - case MODE_3PWM: mode_str = "pwm3"; break; - - case MODE_3PWM1CAP: mode_str = "pwm3cap1"; break; - - case MODE_4PWM: mode_str = "pwm4"; break; - - case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break; - - case MODE_4PWM2CAP: mode_str = "pwm4cap2"; break; - - case MODE_5PWM: mode_str = "pwm5"; break; - - case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break; - - case MODE_6PWM: mode_str = "pwm6"; break; - - case MODE_8PWM: mode_str = "pwm8"; break; - - case MODE_12PWM: mode_str = "pwm12"; break; - - case MODE_14PWM: mode_str = "pwm14"; break; - - case MODE_4CAP: mode_str = "cap4"; break; + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + _mixing_output.printStatus(); - case MODE_5CAP: mode_str = "cap5"; break; + if (_mixing_output.useDynamicMixing() && _pwm_initialized) { + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + if (_timer_rates[timer] >= 0) { + PX4_INFO_RAW("Timer %i: rate: %3i", timer, _timer_rates[timer]); + uint32_t channels = _pwm_mask & up_pwm_servo_get_rate_group(timer); - case MODE_6CAP: mode_str = "cap6"; break; + if (channels > 0) { + PX4_INFO_RAW(" channels: "); - default: - break; - } + for (uint32_t channel = 0; channel < _num_outputs; ++channel) { + if ((1 << channel) & channels) { + PX4_INFO_RAW("%" PRIu32 " ", channel); + } + } + } - if (mode_str) { - PX4_INFO("%d - PWM Mode: %s", _instance, mode_str); + PX4_INFO_RAW("\n"); + } + } } - perf_print_counter(_cycle_perf); - perf_print_counter(_interval_perf); - _mixing_output.printStatus(); - return 0; } @@ -2194,75 +999,21 @@ int PWMOut::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for driving the output and reading the input pins. For boards without a separate IO chip +This module is responsible for driving the output pins. For boards without a separate IO chip (eg. Pixracer), it uses the main channels. On boards with an IO chip (eg. Pixhawk), it uses the AUX channels, and the px4io driver is used for main ones. It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. -The module is configured via mode_* commands. This defines which of the first N pins the driver should occupy. -By using mode_pwm4 for example, pins 5 and 6 can be used by the camera trigger driver or by a PWM rangefinder -driver. Alternatively, pwm_out can be started in one of the capture modes, and then drivers can register a capture -callback with ioctl calls. +On startup, the module tries to occupy all available pins for PWM/Oneshot output. +It skips all pins already in use (e.g. by a camera trigger module). ### Implementation By default the module runs on a work queue with a callback on the uORB actuator_controls topic. - -### Examples -It is typically started with: -$ pwm_out mode_pwm -To drive all available pins. - -Capture input (rising and falling edges) and print on the console: start pwm_out in one of the capture modes: -$ pwm_out mode_pwm3cap1 -This will enable capturing on the 4th pin. Then do: -$ pwm_out test - -Use the `pwm` command for further configurations (PWM rate, levels, ...), and the `mixer` command to load -mixer files. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pwm_out", "driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); - - PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start pwm_out if not running already"); - - PRINT_MODULE_USAGE_COMMAND("mode_gpio"); -#if defined(BOARD_HAS_PWM) - PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); -# if BOARD_HAS_PWM >= 14 - PRINT_MODULE_USAGE_COMMAND("mode_pwm14"); -# endif -# if BOARD_HAS_PWM >= 12 - PRINT_MODULE_USAGE_COMMAND("mode_pwm12"); -# endif -# if BOARD_HAS_PWM >= 8 - PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); -# endif -# if BOARD_HAS_PWM >= 6 - PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm5"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm2"); - PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2"); -# endif - PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); -#endif - - PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)"); - PRINT_MODULE_USAGE_ARG("", "Delay time in ms between reset and re-enabling", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("peripheral_reset", "Reset board peripherals"); - PRINT_MODULE_USAGE_ARG("", "Delay time in ms between reset and re-enabling", true); - - PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate"); - PRINT_MODULE_USAGE_ARG(" ", "Specify the bus id (>=0) and rate in Hz", false); - - PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test inputs and outputs"); + PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -2276,7 +1027,7 @@ extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]) if (strcmp(argv[1], "start") == 0) { - if (_pwm_out_started) { + if (is_running()) { return 0; } @@ -2361,8 +1112,6 @@ extern "C" __EXPORT int pwm_out_main(int argc, char *argv[]) } } - _pwm_out_started = false; - PWMOut::unlock_module(); return PX4_OK; } diff --git a/src/drivers/pwm_out/PWMOut.hpp b/src/drivers/pwm_out/PWMOut.hpp index 1fb7a280b442..87c4f91ebe1b 100644 --- a/src/drivers/pwm_out/PWMOut.hpp +++ b/src/drivers/pwm_out/PWMOut.hpp @@ -37,10 +37,9 @@ #include #include + #include -#include #include -#include #include #include #include @@ -48,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -59,38 +59,14 @@ #include #include #include -#include #include using namespace time_literals; -/** Mode given via CLI */ -enum PortMode { - PORT_MODE_UNSET = 0, - PORT_FULL_GPIO, - PORT_FULL_PWM, - PORT_PWM14, - PORT_PWM12, - PORT_PWM8, - PORT_PWM6, - PORT_PWM5, - PORT_PWM4, - PORT_PWM3, - PORT_PWM2, - PORT_PWM1, - PORT_PWM3CAP1, - PORT_PWM4CAP1, - PORT_PWM4CAP2, - PORT_PWM5CAP1, - PORT_PWM2CAP2, - PORT_CAPTURE, -}; - -#if !defined(BOARD_HAS_PWM) -# error "board_config.h needs to define BOARD_HAS_PWM" +#if !defined(DIRECT_PWM_OUTPUT_CHANNELS) +# error "board_config.h needs to define DIRECT_PWM_OUTPUT_CHANNELS" #endif -// TODO: keep in sync with drivers/camera_capture #define PX4FMU_DEVICE_PATH "/dev/px4fmu" static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1}; @@ -99,27 +75,6 @@ extern pthread_mutex_t pwm_out_module_mutex; class PWMOut : public cdev::CDev, public OutputModuleInterface { public: - enum Mode { - MODE_NONE, - MODE_1PWM, - MODE_2PWM, - MODE_2PWM2CAP, - MODE_3PWM, - MODE_3PWM1CAP, - MODE_4PWM, - MODE_4PWM1CAP, - MODE_4PWM2CAP, - MODE_5PWM, - MODE_5PWM1CAP, - MODE_6PWM, - MODE_8PWM, - MODE_12PWM, - MODE_14PWM, - MODE_4CAP, - MODE_5CAP, - MODE_6CAP, - }; - PWMOut() = delete; explicit PWMOut(int instance = 0, uint8_t output_base = 0); @@ -146,24 +101,17 @@ class PWMOut : public cdev::CDev, public OutputModuleInterface static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); } static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); } - /** change the FMU mode of the running module */ - static int fmu_new_mode(PortMode new_mode); - - static int test(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); + static int test(const char *dev); - virtual int init(); + int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - int set_mode(Mode mode); - Mode get_mode() { return _mode; } - void request_mode(Mode new_mode) { _new_mode_request.store(new_mode); } + int init() override; - static int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); - - static void capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, - uint32_t overflow); + uint32_t get_pwm_mask() const { return _pwm_mask; } + void set_pwm_mask(uint32_t mask) { _pwm_mask = mask; } + uint32_t get_alt_rate_channels() const { return _pwm_alt_rate_channels; } + unsigned get_alt_rate() const { return _pwm_alt_rate; } + unsigned get_default_rate() const { return _pwm_default_rate; } bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; @@ -177,11 +125,9 @@ class PWMOut : public cdev::CDev, public OutputModuleInterface const int _instance; const uint32_t _output_base; - MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; - - Mode _mode{MODE_NONE}; + static const int MAX_PER_INSTANCE{8}; - px4::atomic _new_mode_request{MODE_NONE}; + MixingOutput _mixing_output {PARAM_PREFIX, FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; uint32_t _backup_schedule_interval_us{1_s}; @@ -189,7 +135,9 @@ class PWMOut : public cdev::CDev, public OutputModuleInterface unsigned _pwm_alt_rate{50}; uint32_t _pwm_alt_rate_channels{0}; - unsigned _current_update_rate{0}; + int _timer_rates[MAX_IO_TIMERS] {}; + + int _current_update_rate{0}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -199,28 +147,20 @@ class PWMOut : public cdev::CDev, public OutputModuleInterface bool _pwm_on{false}; uint32_t _pwm_mask{0}; bool _pwm_initialized{false}; - bool _test_mode{false}; unsigned _num_disarmed_set{0}; perf_counter_t _cycle_perf; perf_counter_t _interval_perf; - void capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void update_current_rate(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); - int pwm_ioctl(file *filp, int cmd, unsigned long arg); + int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); - void update_pwm_out_state(bool on); + bool update_pwm_out_state(bool on); void update_params(); - static void sensor_reset(int ms); - static void peripheral_reset(int ms); - - int capture_ioctl(file *filp, int cmd, unsigned long arg); - PWMOut(const PWMOut &) = delete; PWMOut operator=(const PWMOut &) = delete; diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml new file mode 100644 index 000000000000..3f4f380986e9 --- /dev/null +++ b/src/drivers/pwm_out/module.yaml @@ -0,0 +1,33 @@ +module_name: '${PWM_MAIN_OR_AUX}' +actuator_output: + output_groups: + - generator: pwm + param_prefix: '${PWM_MAIN_OR_AUX}' + channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}'] + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } + extra_function_groups: [ pwm_fmu ] + pwm_timer_param: + description: + short: Output Protocol Configuration for ${label} + long: | + Select which Output Protocol to use for outputs ${label}. + + Custom PWM rates can be used by directly setting any value >0. + type: enum + default: 400 + values: + -5: DShot150 + -4: DShot300 + -3: DShot600 + -2: DShot1200 + -1: OneShot + 50: PWM50 + 100: PWM100 + 200: PWM200 + 400: PWM400 + reboot_required: true + diff --git a/src/drivers/pwm_out_sim/CMakeLists.txt b/src/drivers/pwm_out_sim/CMakeLists.txt index 77a5de2543d7..49e6a871178c 100644 --- a/src/drivers/pwm_out_sim/CMakeLists.txt +++ b/src/drivers/pwm_out_sim/CMakeLists.txt @@ -30,13 +30,21 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + +if (${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL} STREQUAL "px4_sitl") + set(module_config "module_sim.yaml") +else() + set(module_config "module_hil.yaml") +endif() + px4_add_module( MODULE drivers__pwm_out_sim MAIN pwm_out_sim SRCS PWMSim.cpp + MODULE_CONFIG + ${module_config} DEPENDS mixer mixer_module - output_limit ) diff --git a/src/drivers/pwm_out_sim/Kconfig b/src/drivers/pwm_out_sim/Kconfig new file mode 100644 index 000000000000..49d187b40045 --- /dev/null +++ b/src/drivers/pwm_out_sim/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PWM_OUT_SIM + bool "pwm_out_sim" + default n + ---help--- + Enable support for pwm_out_sim \ No newline at end of file diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 86d2f57cdd52..cc45d54ab411 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * 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 @@ -39,6 +39,8 @@ #include #include +#include + PWMSim::PWMSim(bool hil_mode_enabled) : CDev(PWM_OUTPUT0_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) @@ -64,6 +66,8 @@ PWMSim::Run() return; } + SmartLock lock_guard(_lock); + _mixing_output.update(); // check for parameter updates @@ -81,17 +85,48 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - // Nothing to do, as we are only interested in the actuator_outputs topic publication. - // That should only be published once we receive actuator_controls (important for lock-step to work correctly) - return num_control_groups_updated > 0; + // Only publish once we receive actuator_controls (important for lock-step to work correctly) + if (num_control_groups_updated > 0) { + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = num_outputs; + + const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); + + for (int i = 0; i < (int)num_outputs; i++) { + if (outputs[i] != PWM_SIM_DISARMED_MAGIC) { + + OutputFunction function = _mixing_output.outputFunction(i); + bool is_reversible = reversible_outputs & (1u << i); + float output = outputs[i]; + + if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax) + && !is_reversible) { + // Scale non-reversible motors to [0, 1] + actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC); + + } else { + // Scale everything else to [-1, 1] + const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f; + const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f; + actuator_outputs.output[i] = (output - pwm_center) / pwm_delta; + } + } + } + + actuator_outputs.timestamp = hrt_absolute_time(); + _actuator_outputs_sim_pub.publish(actuator_outputs); + return true; + } + + return false; } int PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) { - int ret = OK; + SmartLock lock_guard(_lock); - lock(); + int ret = OK; switch (cmd) { case PWM_SERVO_ARM: @@ -104,7 +139,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS) { + if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) { _mixing_output.minValue(i) = pwm->values[i]; } } @@ -116,7 +151,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; for (unsigned i = 0; i < pwm->channel_count; i++) { - if (i < OutputModuleInterface::MAX_ACTUATORS) { + if (i < OutputModuleInterface::MAX_ACTUATORS && !_mixing_output.useDynamicMixing()) { _mixing_output.maxValue(i) = pwm->values[i]; } } @@ -176,17 +211,6 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET_TRIM_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - for (unsigned i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { - pwm->values[i] = (_mixing_output.maxValue(i) + _mixing_output.minValue(i)) / 2; - } - - pwm->channel_count = OutputModuleInterface::MAX_ACTUATORS; - break; - } - case PWM_SERVO_GET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; @@ -211,24 +235,21 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) break; case MIXERIOCRESET: - _mixing_output.resetMixerThreadSafe(); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strlen(buf); - ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + ret = _mixing_output.loadMixer(buf, buflen); break; } - default: ret = -ENOTTY; break; } - unlock(); - return ret; } diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index 42a2d5dde42a..4ad12dd57b6d 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -43,6 +43,16 @@ #include #include #include +#include +#include +#include + +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) +#define PARAM_PREFIX "PWM_MAIN" +#else +#define PARAM_PREFIX "HIL_ACT" +#endif + using namespace time_literals; @@ -77,7 +87,9 @@ class PWMSim : public cdev::CDev, public ModuleBase, public OutputModule static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000; static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000; - MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)}; }; diff --git a/src/drivers/pwm_out_sim/module_hil.yaml b/src/drivers/pwm_out_sim/module_hil.yaml new file mode 100644 index 000000000000..25b9db39e02b --- /dev/null +++ b/src/drivers/pwm_out_sim/module_hil.yaml @@ -0,0 +1,8 @@ + +module_name: HIL +actuator_output: + show_subgroups_if: 'SYS_HITL>0' + output_groups: + - param_prefix: HIL_ACT + channel_label: Channel + num_channels: 16 diff --git a/src/drivers/pwm_out_sim/module_sim.yaml b/src/drivers/pwm_out_sim/module_sim.yaml new file mode 100644 index 000000000000..9964f0c419d2 --- /dev/null +++ b/src/drivers/pwm_out_sim/module_sim.yaml @@ -0,0 +1,7 @@ + +module_name: SIM +actuator_output: + output_groups: + - param_prefix: PWM_MAIN + channel_label: Channel + num_channels: 16 diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index df9dbdde43d6..6ad4f44ef29c 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -39,40 +39,38 @@ px4_add_module( px4io.cpp px4io_serial.cpp px4io_uploader.cpp + MODULE_CONFIG + module.yaml DEPENDS arch_px4io_serial + button_publisher circuit_breaker - mixer + mixer_module ) # include the px4io binary in ROMFS -message(STATUS "Building and including ${config_io_board}") +if(CONFIG_BOARD_IO) + message(STATUS "drivers/px4io: ROMFS including ${CONFIG_BOARD_IO}") -include(ExternalProject) -ExternalProject_Add(px4io_firmware - SOURCE_DIR ${CMAKE_SOURCE_DIR} - DOWNLOAD_COMMAND "" - UPDATE_COMMAND "" - CMAKE_ARGS -DCONFIG=${config_io_board} - INSTALL_COMMAND "" - USES_TERMINAL_BUILD true - DEPENDS git_nuttx git_nuttx_apps - BUILD_ALWAYS 1 - BUILD_BYPRODUCTS "${PX4_BINARY_DIR}/external/Build/px4io_firmware/${config_io_board}.elf" -) + include(ExternalProject) -ExternalProject_Get_Property(px4io_firmware BINARY_DIR) - -set(fw_io_exe "${BINARY_DIR}/${config_io_board}.elf") -set(fw_io_bin "${PX4_BINARY_DIR}/romfs_extras/${config_io_board}.bin" CACHE FILEPATH "px4io binary path") + ExternalProject_Add(px4io_firmware_build + DOWNLOAD_COMMAND rsync -a --safe-links --delete --exclude=build --exclude=*.o --exclude=*.a --exclude=.config ${PX4_SOURCE_DIR}/ ${PX4_BINARY_DIR}/external/Source/px4io_firmware_build/ + UPDATE_COMMAND rsync -a --safe-links --delete --exclude=build --exclude=*.o --exclude=*.a --exclude=.config ${PX4_SOURCE_DIR}/ ${PX4_BINARY_DIR}/external/Source/px4io_firmware_build/ + DEPENDS git_nuttx git_nuttx_apps + SOURCE_DIR ${PX4_BINARY_DIR}/external/Source/px4io_firmware_build/ + CMAKE_ARGS -DCONFIG=${CONFIG_BOARD_IO} + INSTALL_COMMAND "" + USES_TERMINAL_BUILD true + EXCLUDE_FROM_ALL true + BUILD_ALWAYS true + ) -file(RELATIVE_PATH fw_io_exe_relative ${CMAKE_CURRENT_BINARY_DIR} ${fw_io_exe}) -file(RELATIVE_PATH fw_io_bin_relative ${CMAKE_CURRENT_BINARY_DIR} ${fw_io_bin}) + ExternalProject_Get_Property(px4io_firmware_build BINARY_DIR) -add_custom_command(OUTPUT ${fw_io_bin} - COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/romfs_extras - COMMAND ${CMAKE_OBJCOPY} -O binary ${fw_io_exe_relative} ${fw_io_bin_relative} - DEPENDS ${fw_io_exe} px4io_firmware - COMMENT "Copying ${config_io_board} to ROMFS extras" + add_custom_target(px4io_firmware_update + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${BINARY_DIR}/${CONFIG_BOARD_IO}.bin ${PX4_BOARD_DIR}/extras/${CONFIG_BOARD_IO}.bin + DEPENDS px4io_firmware_build + COMMENT "Copying ${CONFIG_BOARD_IO} to board extras" ) -add_custom_target(copy_px4io_bin DEPENDS ${fw_io_bin}) +endif() diff --git a/src/drivers/px4io/Kconfig b/src/drivers/px4io/Kconfig new file mode 100644 index 000000000000..dd9b402249fa --- /dev/null +++ b/src/drivers/px4io/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_PX4IO + bool "px4io" + default n + ---help--- + Enable support for px4io diff --git a/src/drivers/px4io/module.yaml b/src/drivers/px4io/module.yaml new file mode 100644 index 000000000000..fcbde93c29f4 --- /dev/null +++ b/src/drivers/px4io/module.yaml @@ -0,0 +1,29 @@ +module_name: PWM MAIN +actuator_output: + output_groups: + - generator: pwm + param_prefix: PWM_MAIN + channel_labels: ['MAIN', 'Capture'] + channel_label_module_name_prefix: false + timer_config_file: "boards/px4/io-v2/src/timer_config.cpp" + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } + pwm_timer_param: + description: + short: Output Protocol Configuration for ${label} + long: | + Select which Output Protocol to use for outputs ${label}. + + Custom PWM rates can be used by directly setting any value >0. + type: enum + default: 400 + values: + -1: OneShot + 50: PWM50 + 100: PWM100 + 200: PWM200 + 400: PWM400 + reboot_required: true diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1a89fa43fcca..f16f81c25624 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * 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 @@ -37,64 +37,46 @@ * * PX4IO is connected via DMA enabled high-speed UART. */ - -#include -#include +#include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include - - #include -#include -#include -#include #include #include #include +#include +#include -#include - +#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include +#include #include +#include #include #include -#include -#include -#include +#include #include #include -#include +#include #include #include -#include -#include #include @@ -107,12 +89,10 @@ #include "px4io_driver.h" #define PX4IO_SET_DEBUG _IOC(0xff00, 0) -#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) -#define PX4IO_CHECK_CRC _IOC(0xff00, 3) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 1) +#define PX4IO_CHECK_CRC _IOC(0xff00, 2) -#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz -#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz +static constexpr unsigned MIN_TOPIC_UPDATE_INTERVAL = 2500; // 2.5 ms -> 400 Hz using namespace time_literals; @@ -121,7 +101,7 @@ using namespace time_literals; * * Encapsulates PX4FMU to PX4IO communications modeled as file operations. */ -class PX4IO : public cdev::CDev +class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleInterface { public: /** @@ -129,38 +109,17 @@ class PX4IO : public cdev::CDev * * Initialize all class variables. */ - PX4IO(device::Device *interface); - - /** - * Destructor. - * - * Wait for worker thread to terminate. - */ - virtual ~PX4IO(); + PX4IO() = delete; + explicit PX4IO(device::Device *interface); - /** - * Initialize the PX4IO class. - * - * Retrieve relevant initial system parameters. Initialize PX4IO registers. - */ - virtual int init(); + ~PX4IO() override; /** * Initialize the PX4IO class. * * Retrieve relevant initial system parameters. Initialize PX4IO registers. - * - * @param disable_rc_handling set to true to forbid override / RC handling on IO - * @param hitl_mode set to suppress publication of actuator_outputs - instead defer to pwm_out_sim - */ - int init(bool disable_rc_handling, bool hitl_mode); - - /** - * Detect if a PX4IO is connected. - * - * Only validate if there is a PX4IO to talk to. */ - virtual int detect(); + int init() override; /** * IO Control handler. @@ -171,61 +130,64 @@ class PX4IO : public cdev::CDev * @param[in] cmd the IOCTL command * @param[in] the IOCTL command parameter (optional) */ - virtual int ioctl(file *filp, int cmd, unsigned long arg); - - /** - * Disable RC input handling - */ - int disable_rc_handling(); + int ioctl(file *filp, int cmd, unsigned long arg) override; /** * Print IO status. * * Print all relevant IO status information * - * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(bool extended_status); + int print_status(); - /** - * Fetch and print debug console output. - */ - int print_debug(); + static int custom_command(int argc, char *argv[]); + + static int print_usage(const char *reason = nullptr); + + static int task_spawn(int argc, char *argv[]); /* * To test what happens if IO stops receiving updates from FMU. * * @param is_fail true for failure condition, false for normal operation. */ - void test_fmu_fail(bool is_fail) - { - _test_fmu_fail = is_fail; - }; + void test_fmu_fail(bool is_fail) { _test_fmu_fail = is_fail; }; + + uint16_t system_status() const { return _status; } - inline uint16_t system_status() const {return _status;} + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) override; private: - device::Device *_interface; + void Run() override; + + void updateDisarmed(); + void updateFailsafe(); + void updateTimerRateGroups(); + + static int checkcrc(int argc, char *argv[]); + static int bind(int argc, char *argv[]); - unsigned _hardware; ///< Hardware revision - unsigned _max_actuators; ///< Maximum # of actuators supported by PX4IO - unsigned _max_controls; ///< Maximum # of controls supported by PX4IO - unsigned _max_rc_input; ///< Maximum receiver channels supported by PX4IO - unsigned _max_relays; ///< Maximum relays supported by PX4IO - unsigned _max_transfer; ///< Maximum number of I2C transfers supported by PX4IO + static constexpr int PX4IO_MAX_ACTUATORS = 8; - bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values - unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels - uint64_t _rc_last_valid; ///< last valid timestamp + device::Device *const _interface; - volatile int _task; ///< worker task id - volatile bool _task_should_exit; ///< worker terminate flag + unsigned _hardware{0}; ///< Hardware revision + unsigned _max_actuators{0}; ///< Maximum # of actuators supported by PX4IO + unsigned _max_controls{0}; ///< Maximum # of controls supported by PX4IO + unsigned _max_rc_input{0}; ///< Maximum receiver channels supported by PX4IO + unsigned _max_transfer{16}; ///< Maximum number of I2C transfers supported by PX4IO - orb_advert_t _mavlink_log_pub; ///< mavlink log pub + int _class_instance{-1}; - perf_counter_t _perf_update; ///< local performance counter for status updates - perf_counter_t _perf_write; ///< local performance counter for PWM control writes - perf_counter_t _perf_sample_latency; ///< total system latency (based on passed-through timestamp) + hrt_abstime _poll_last{0}; + + orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _interface_read_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface read")}; + perf_counter_t _interface_write_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": interface write")}; /* cached IO state */ uint16_t _status{0}; ///< Various IO status flags @@ -234,86 +196,48 @@ class PX4IO : public cdev::CDev uint16_t _last_written_arming_s{0}; ///< the last written arming state reg uint16_t _last_written_arming_c{0}; ///< the last written arming state reg - /* subscribed topics */ - int _t_actuator_controls_0; ///< actuator controls group 0 topic - - uORB::Subscription _t_actuator_controls_1{ORB_ID(actuator_controls_1)}; ///< actuator controls group 1 topic - uORB::Subscription _t_actuator_controls_2{ORB_ID(actuator_controls_2)};; ///< actuator controls group 2 topic - uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic - uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic - uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic + uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic + uORB::Subscription _t_vehicle_status{ORB_ID(vehicle_status)}; ///< vehicle status topic uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; hrt_abstime _last_status_publish{0}; - bool _param_update_force; ///< force a parameter update + uint16_t _rc_valid_update_count{0}; + + bool _param_update_force{true}; ///< force a parameter update + bool _timer_rates_configured{false}; /* advertised topics */ - uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; - uORB::PublicationMulti _to_outputs{ORB_ID(actuator_outputs)}; - uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; - uORB::Publication _px4io_status_pub{ORB_ID(px4io_status)}; - uORB::Publication _to_safety{ORB_ID(safety)}; - - safety_s _safety{}; - - bool _primary_pwm_device; ///< true if we are the default PWM output - bool _lockdown_override; ///< allow to override the safety lockdown - bool _armed; ///< wether the system is armed - bool _override_available; ///< true if manual reversion mode is enabled - - bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled - bool _in_esc_calibration_mode; ///< do not send control outputs to IO (used for esc calibration) - - int32_t _rssi_pwm_chan; ///< RSSI PWM input channel - int32_t _rssi_pwm_max; ///< max RSSI input on PWM channel - int32_t _rssi_pwm_min; ///< min RSSI input on PWM channel - int32_t _thermal_control; ///< thermal control state - bool _analog_rc_rssi_stable; ///< true when analog RSSI input is stable - float _analog_rc_rssi_volt; ///< analog RSSI voltage - - bool _test_fmu_fail; ///< To test what happens if IO loses FMU - - struct MotorTest { - uORB::Subscription test_motor_sub{ORB_ID(test_motor)}; - bool in_test_mode{false}; - hrt_abstime timeout{0}; - }; - MotorTest _motor_test; - bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs + uORB::PublicationMulti _to_input_rc{ORB_ID(input_rc)}; + uORB::Publication _px4io_status_pub{ORB_ID(px4io_status)}; - /** - * Trampoline to the worker task - */ - static int task_main_trampoline(int argc, char *argv[]); + ButtonPublisher _button_publisher; + bool _previous_safety_off{false}; - /** - * worker task - */ - void task_main(); + bool _lockdown_override{false}; ///< override the safety lockdown - /** - * Send controls for one group to IO - */ - int io_set_control_state(unsigned group); + int32_t _thermal_control{-1}; ///< thermal control state + bool _analog_rc_rssi_stable{false}; ///< true when analog RSSI input is stable + float _analog_rc_rssi_volt{-1.f}; ///< analog RSSI voltage - /** - * Send all controls to IO - */ - int io_set_control_groups(); + bool _test_fmu_fail{false}; ///< To test what happens if IO loses FMU + bool _in_test_mode{false}; ///< true if PWM_SERVO_ENTER_TEST_MODE is active + + MixingOutput _mixing_output{"PWM_MAIN", PX4IO_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + + bool _pwm_min_configured{false}; + bool _pwm_max_configured{false}; + bool _pwm_fail_configured{false}; + bool _pwm_dis_configured{false}; + bool _pwm_rev_configured{false}; /** * Update IO's arming-related state */ int io_set_arming_state(); - /** - * Push RC channel configuration to IO. - */ - int io_set_rc_config(); - /** * Fetch status and alarms from IO * @@ -321,29 +245,14 @@ class PX4IO : public cdev::CDev */ int io_get_status(); - /** - * Disable RC input handling - */ - int io_disable_rc_handling(); - /** * Fetch RC inputs from IO. * * @param input_rc Input structure to populate. * @return OK if data was returned. */ - int io_get_raw_rc_input(input_rc_s &input_rc); - - /** - * Fetch and publish raw RC input data. - */ int io_publish_raw_rc(); - /** - * Fetch and publish the PWM servo outputs. - */ - int io_publish_pwm_outputs(); - /** * write register(s) * @@ -427,156 +336,68 @@ class PX4IO : public cdev::CDev void update_params(); - /** - * check and handle test_motor topic updates - */ - void handle_motor_test(); - - /* do not allow to copy this class due to ptr data members */ - PX4IO(const PX4IO &); - PX4IO operator=(const PX4IO &); + DEFINE_PARAMETERS( + (ParamInt) _param_pwm_sbus_mode, + (ParamInt) _param_rc_rssi_pwm_chan, + (ParamInt) _param_rc_rssi_pwm_max, + (ParamInt) _param_rc_rssi_pwm_min, + (ParamInt) _param_sens_en_themal, + (ParamInt) _param_sys_hitl, + (ParamInt) _param_sys_use_io + ) }; -namespace -{ -PX4IO *g_dev = nullptr; -} - #define PX4IO_DEVICE_PATH "/dev/px4io" PX4IO::PX4IO(device::Device *interface) : CDev(PX4IO_DEVICE_PATH), - _interface(interface), - _hardware(0), - _max_actuators(0), - _max_controls(0), - _max_rc_input(0), - _max_relays(0), - _max_transfer(16), /* sensible default */ - _rc_handling_disabled(false), - _rc_chan_count(0), - _rc_last_valid(0), - _task(-1), - _task_should_exit(false), - _mavlink_log_pub(nullptr), - _perf_update(perf_alloc(PC_ELAPSED, "io update")), - _perf_write(perf_alloc(PC_ELAPSED, "io write")), - _perf_sample_latency(perf_alloc(PC_ELAPSED, "io control latency")), - _t_actuator_controls_0(-1), - _param_update_force(false), - _primary_pwm_device(false), - _lockdown_override(false), - _armed(false), - _override_available(false), - _cb_flighttermination(true), - _in_esc_calibration_mode(false), - _rssi_pwm_chan(0), - _rssi_pwm_max(0), - _rssi_pwm_min(0), - _thermal_control(-1), - _analog_rc_rssi_stable(false), - _analog_rc_rssi_volt(-1.0f), - _test_fmu_fail(false), - _hitl_mode(false) + OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(PX4IO_SERIAL_DEVICE)), + _interface(interface) { - /* we need this potentially before it could be set in task_main */ - g_dev = this; + if (!_mixing_output.useDynamicMixing()) { + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); + } + + _mixing_output.setLowrateSchedulingInterval(20_ms); } PX4IO::~PX4IO() { - /* tell the task we want it to go away */ - _task_should_exit = true; - - /* spin waiting for the task to stop */ - for (unsigned i = 0; (i < 10) && (_task != -1); i++) { - /* give it another 100ms */ - px4_usleep(100000); - } + delete _interface; - /* well, kill it anyway, though this will probably crash */ - if (_task != -1) { - task_delete(_task); - } - - if (_interface != nullptr) { - delete _interface; + /* clean up the alternate device node */ + if (_class_instance >= 0) { + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); } /* deallocate perfs */ - perf_free(_perf_update); - perf_free(_perf_write); - perf_free(_perf_sample_latency); - - g_dev = nullptr; + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_interface_read_perf); + perf_free(_interface_write_perf); } -int -PX4IO::detect() +bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) { - int ret; - - if (_task == -1) { - - /* do regular cdev init */ - ret = CDev::init(); - - if (ret != OK) { - return ret; - } - - /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); - - if (protocol != PX4IO_PROTOCOL_VERSION) { - if (protocol == _io_reg_get_error) { - PX4_ERR("IO not installed"); - - } else { - PX4_ERR("IO version error"); - mavlink_log_emergency(&_mavlink_log_pub, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); - } - - return -1; - } + if (!_test_fmu_fail) { + /* output to the servos */ + io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); } - PX4_INFO("IO found"); - - return 0; -} - -int -PX4IO::init(bool rc_handling_disabled, bool hitl_mode) -{ - _rc_handling_disabled = rc_handling_disabled; - _hitl_mode = hitl_mode; - return init(); + return true; } -int -PX4IO::init() +int PX4IO::init() { - int ret; - param_t sys_restart_param; - int32_t sys_restart_val = DM_INIT_REASON_VOLATILE; - - sys_restart_param = param_find("SYS_RESTART_TYPE"); - - if (sys_restart_param != PARAM_INVALID) { - /* Indicate restart type is unknown */ - int32_t prev_val; - param_get(sys_restart_param, &prev_val); - - if (prev_val != DM_INIT_REASON_POWER_ON) { - param_set_no_notification(sys_restart_param, &sys_restart_val); - } - } + SmartLock lock_guard(_lock); /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { + PX4_ERR("init failed %d", ret); return ret; } @@ -591,60 +412,52 @@ PX4IO::init() /* if the error still persists after timing out, we give up */ if (protocol == _io_reg_get_error) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort."); + mavlink_log_emergency(&_mavlink_log_pub, "Failed to communicate with IO, abort.\t"); + events::send(events::ID("px4io_comm_failed"), events::Log::Emergency, + "Failed to communicate with IO, aborting initialization"); return -1; } if (protocol != PX4IO_PROTOCOL_VERSION) { - mavlink_log_emergency(&_mavlink_log_pub, "IO protocol/firmware mismatch, abort."); + mavlink_log_emergency(&_mavlink_log_pub, "IO protocol/firmware mismatch, abort.\t"); + events::send(events::ID("px4io_proto_fw_mismatch"), events::Log::Emergency, + "IO protocol/firmware mismatch, aborting initialization"); return -1; } _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); - _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); - if ((_max_actuators < 1) || (_max_actuators > 16) || - (_max_relays > 32) || + if ((_max_actuators < 1) || (_max_actuators > PX4IO_MAX_ACTUATORS) || (_max_transfer < 16) || (_max_transfer > 255) || (_max_rc_input < 1) || (_max_rc_input > 255)) { PX4_ERR("config read error"); - mavlink_log_emergency(&_mavlink_log_pub, "[IO] config read fail, abort."); + mavlink_log_emergency(&_mavlink_log_pub, "[IO] config read fail, abort.\t"); + events::send(events::ID("px4io_config_read_failed"), events::Log::Emergency, + "IO config read failed, aborting initialization"); // ask IO to reboot into bootloader as the failure may // be due to mismatched firmware versions and we want // the startup script to be able to load a new IO // firmware - // If IO has already safety off it won't accept going into bootloader mode, - // therefore we need to set safety on first. - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); - // Now the reboot into bootloader mode should succeed. io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC); return -1; } + /* Set safety_off to false when FMU boot*/ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0); + if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) { _max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS; } - param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); - param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); - param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); - - /* - * Check for IO flight state - if FMU was flagged to be in - * armed state, FMU is recovering from an in-air reset. - * Read back status and request the commander to arm - * in this case. - */ - - uint16_t reg; + uint16_t reg = 0; /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); @@ -653,675 +466,389 @@ PX4IO::init() return ret; } - /* - * in-air restart is only tried if the IO board reports it is - * already armed, and has been configured for in-air restart - */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - - /* get a status update from IO */ - io_get_status(); + /* dis-arm IO before touching anything */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_LOCKDOWN, + 0); - mavlink_log_emergency(&_mavlink_log_pub, "RECOVERING FROM FMU IN-AIR RESTART"); + if (ret != OK) { + mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail\t"); + events::send(events::ID("px4io_io_rc_config_upload_failed"), events::Log::Critical, + "IO RC config upload failed, aborting initialization"); + return ret; + } - /* WARNING: COMMANDER app/vehicle status must be initialized. - * If this fails (or the app is not started), worst-case IO - * remains untouched (so manual override is still available). - */ + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { + _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); + _mixing_output.setDriverInstance(_class_instance); - uORB::Subscription actuator_armed_sub{ORB_ID(actuator_armed)}; + _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); + } - /* fill with initial values, clear updated flag */ - actuator_armed_s actuator_armed{}; - uint64_t try_start_time = hrt_absolute_time(); + _px4io_status_pub.advertise(); - /* keep checking for an update, ensure we got a arming information, - not something that was published a long time ago. */ - do { - if (actuator_armed_sub.update(&actuator_armed)) { - // updated data, exit loop - break; - } + update_params(); - /* wait 10 ms */ - px4_usleep(10000); + ScheduleNow(); - /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (1), abort"); - return 1; - } + return OK; +} - } while (true); +void PX4IO::updateDisarmed() +{ + pwm_output_values pwm{}; - /* send this to itself */ - param_t sys_id_param = param_find("MAV_SYS_ID"); - param_t comp_id_param = param_find("MAV_COMP_ID"); + for (unsigned i = 0; i < _max_actuators; i++) { + pwm.values[i] = _mixing_output.disarmedValue(i); + } - int32_t sys_id; - int32_t comp_id; + io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, _max_actuators); +} - if (param_get(sys_id_param, &sys_id)) { - errx(1, "PRM SYSID"); - } +void PX4IO::updateFailsafe() +{ + pwm_output_values pwm{}; - if (param_get(comp_id_param, &comp_id)) { - errx(1, "PRM CMPID"); - } + for (unsigned i = 0; i < _max_actuators; i++) { + pwm.values[i] = _mixing_output.actualFailsafeValue(i); + } - /* prepare vehicle command */ - vehicle_command_s vcmd = {}; - vcmd.target_system = (uint8_t)sys_id; - vcmd.target_component = (uint8_t)comp_id; - vcmd.source_system = (uint8_t)sys_id; - vcmd.source_component = (uint8_t)comp_id; - vcmd.confirmation = true; /* ask to confirm command */ - - if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) { - mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe"); - /* send command to terminate flight via command API */ - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = 1.0f; /* request flight termination */ - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; - - /* send command once */ - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); - - /* spin here until IO's state has propagated into the system */ - do { - actuator_armed_sub.update(&actuator_armed); - - /* wait 50 ms */ - px4_usleep(50000); - - /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort"); - return 1; - } + io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, _max_actuators); +} - /* re-send if necessary */ - if (!actuator_armed.force_failsafe) { - vcmd_pub.publish(vcmd); - PX4_WARN("re-sending flight termination cmd"); - } +void PX4IO::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); - /* keep waiting for state change for 2 s */ - } while (!actuator_armed.force_failsafe); - } + exit_and_cleanup(); + return; + } - /* send command to arm system via command API */ - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = 1.0f; /* request arming */ - vcmd.param3 = 1234.f; /* mark the command coming from IO (for in-air restoring) */ - vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; + SmartLock lock_guard(_lock); - /* send command once */ - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; - vcmd_pub.publish(vcmd); + perf_begin(_cycle_perf); + perf_count(_interval_perf); - /* spin here until IO's state has propagated into the system */ - do { - actuator_armed_sub.update(&actuator_armed); + // schedule minimal update rate if there are no actuator controls + if (!_mixing_output.useDynamicMixing()) { + ScheduleDelayed(20_ms); + } - /* wait 50 ms */ - px4_usleep(50000); + /* if we have new control data from the ORB, handle it */ + if (_param_sys_hitl.get() <= 0) { + _mixing_output.update(); + } - /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { - mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (2), abort"); - return 1; - } + if (hrt_elapsed_time(&_poll_last) >= 20_ms) { + /* run at 50 */ + _poll_last = hrt_absolute_time(); - /* re-send if necessary */ - if (!actuator_armed.armed) { - vcmd_pub.publish(vcmd); - PX4_WARN("re-sending arm cmd"); - } + /* pull status and alarms from IO */ + io_get_status(); - /* keep waiting for state change for 2 s */ - } while (!actuator_armed.armed); + /* get raw R/C input from IO */ + io_publish_raw_rc(); + } - /* Indicate restart type is in-flight */ - sys_restart_val = DM_INIT_REASON_IN_FLIGHT; - int32_t prev_val; - param_get(sys_restart_param, &prev_val); + if (_param_sys_hitl.get() <= 0) { + /* check updates on uORB topics and handle it */ + if (_t_actuator_armed.updated()) { + io_set_arming_state(); - if (prev_val != sys_restart_val) { - param_set(sys_restart_param, &sys_restart_val); + // TODO: throttle } + } - /* regular boot, no in-air restart, init IO */ + if (!_mixing_output.armed().armed) { + /* vehicle command */ + if (_t_vehicle_command.updated()) { + vehicle_command_s cmd{}; + _t_vehicle_command.copy(&cmd); - } else { + // Check for a DSM pairing command + if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { + int bind_arg; - /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_FMU_ARMED | - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | - PX4IO_P_SETUP_ARMING_LOCKDOWN, 0); + switch ((int)cmd.param2) { + case 0: + bind_arg = DSM2_BIND_PULSES; + break; - if (_rc_handling_disabled) { - ret = io_disable_rc_handling(); + case 1: + bind_arg = DSMX_BIND_PULSES; + break; - if (ret != OK) { - PX4_ERR("failed disabling RC handling"); - return ret; - } + case 2: + default: + bind_arg = DSMX8_BIND_PULSES; + break; + } - } else { - /* publish RC config to IO */ - ret = io_set_rc_config(); + int dsm_ret = dsm_bind_ioctl(bind_arg); - if (ret != OK) { - mavlink_log_critical(&_mavlink_log_pub, "IO RC config upload fail"); - return ret; + /* publish ACK */ + if (dsm_ret == OK) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + } } } - /* Indicate restart type is power on */ - sys_restart_val = DM_INIT_REASON_POWER_ON; - int32_t prev_val; - param_get(sys_restart_param, &prev_val); + /* + * If parameters have changed, re-send RC mappings to IO + */ - if (prev_val != sys_restart_val) { - param_set(sys_restart_param, &sys_restart_val); - } + // check for parameter updates + if (_parameter_update_sub.updated() || _param_update_force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - } + _param_update_force = false; - /* set safety to off if circuit breaker enabled */ - if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); - } + ModuleParams::updateParams(); - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT0_DEVICE_PATH, &fops, 0666, (void *)this); + update_params(); - if (ret == OK) { - PX4_INFO("default PWM output device"); - _primary_pwm_device = true; - } + /* Check if the flight termination circuit breaker has been updated */ + bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + /* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !disable_flighttermination); - /* start the IO interface task */ - _task = px4_task_spawn_cmd("px4io", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1500, - (px4_main_t)&PX4IO::task_main_trampoline, - nullptr); + if (_param_sens_en_themal.get() != _thermal_control || _param_update_force) { - if (_task < 0) { - PX4_ERR("task start failed: %d", errno); - return -errno; - } + _thermal_control = _param_sens_en_themal.get(); + /* set power management state for thermal */ + uint16_t tctrl; - return OK; -} + if (_thermal_control < 0) { + tctrl = PX4IO_THERMAL_IGNORE; -int -PX4IO::task_main_trampoline(int argc, char *argv[]) -{ - g_dev->task_main(); - return 0; -} + } else { + tctrl = PX4IO_THERMAL_OFF; + } -void -PX4IO::task_main() -{ - hrt_abstime poll_last = 0; - hrt_abstime orb_check_last = 0; + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); + } - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0)); - orb_set_interval(_t_actuator_controls_0, 2); /* default to 500Hz */ + /* S.BUS output */ + if (_param_pwm_sbus_mode.get() == 1) { + /* enable S.BUS 1 */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); - if (_t_actuator_controls_0 < 0) { - PX4_ERR("actuator subscription failed"); - goto out; - } + } else if (_param_pwm_sbus_mode.get() == 2) { + /* enable S.BUS 2 */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - /* Fetch initial flight termination circuit breaker state */ - _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } else { + /* disable S.BUS */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, + (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + } + } - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _t_actuator_controls_0; - fds[0].events = POLLIN; + _mixing_output.updateSubscriptions(true, true); - _param_update_force = true; + perf_end(_cycle_perf); +} - /* lock against the ioctl handler */ - lock(); +void PX4IO::updateTimerRateGroups() +{ + if (_timer_rates_configured) { // avoid setting timer rates on each param update + return; + } - /* loop talking to IO */ - while (!_task_should_exit) { + _timer_rates_configured = true; - /* sleep waiting for topic updates, but no more than 20ms */ - unlock(); - int ret = ::poll(fds, 1, 20); - lock(); + int timer = 0; - /* this would be bad... */ - if (ret < 0) { - warnx("poll error %d", errno); - continue; - } + uint16_t timer_rates[PX4IO_P_SETUP_PWM_RATE_GROUP3 - PX4IO_P_SETUP_PWM_RATE_GROUP0 + 1] {}; - perf_begin(_perf_update); - hrt_abstime now = hrt_absolute_time(); + for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) { + int32_t tim_config = 0; + param_t handle = param_find(param_name); - /* we're not nice to the lower-priority control groups and only check them - when the primary group updated (which is now). */ - (void)io_set_control_groups(); + if (handle == PARAM_INVALID) { + break; } - if (!_armed && !_lockdown_override) { - handle_motor_test(); + param_get(handle, &tim_config); - } else { - _motor_test.in_test_mode = false; + if (tim_config > 0) { + timer_rates[timer] = tim_config; + + } else if (tim_config == -1) { // OneShot + timer_rates[timer] = 0; } - if (now >= poll_last + IO_POLL_INTERVAL) { - /* run at 50-250Hz */ - poll_last = now; + ++timer; + } - /* pull status and alarms from IO */ - io_get_status(); + int ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATE_GROUP0, timer_rates, timer); - /* get raw R/C input from IO */ - io_publish_raw_rc(); + if (ret != 0) { + PX4_ERR("io_reg_set failed (%i)", ret); + } +} - /* fetch PWM outputs from IO */ - io_publish_pwm_outputs(); +void PX4IO::update_params() +{ + if (!_mixing_output.armed().armed && _mixing_output.useDynamicMixing()) { + // sync params to IO + updateTimerRateGroups(); + updateFailsafe(); + updateDisarmed(); + return; + } - /* check updates on uORB topics and handle it */ - bool updated = false; + // skip update when armed or PWM disabled + if (_mixing_output.armed().armed || _class_instance == -1 || _mixing_output.useDynamicMixing()) { + return; + } - /* arming state */ - updated = _t_actuator_armed.updated(); + int32_t pwm_min_default = PWM_DEFAULT_MIN; + int32_t pwm_max_default = PWM_DEFAULT_MAX; + int32_t pwm_disarmed_default = 0; + int32_t pwm_rate_default = 50; + int32_t pwm_default_channels = 0; - if (!updated) { - updated = _t_vehicle_control_mode.updated(); - } + const char *prefix = "PWM_MAIN"; - if (updated) { - io_set_arming_state(); - } - } + param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default); + param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); + param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); + param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default); + param_get(param_find("PWM_MAIN_OUT"), &pwm_default_channels); - if (!_armed && (now >= orb_check_last + ORB_CHECK_INTERVAL)) { - /* run at 5Hz */ - orb_check_last = now; + uint32_t single_ch = 0; + uint32_t pwm_default_channel_mask = 0; - /* vehicle command */ - if (_t_vehicle_command.updated()) { - vehicle_command_s cmd{}; - _t_vehicle_command.copy(&cmd); + while ((single_ch = pwm_default_channels % 10)) { + pwm_default_channel_mask |= 1 << (single_ch - 1); + pwm_default_channels /= 10; + } - // Check for a DSM pairing command - if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) { - int bind_arg; + char str[17]; - switch ((int)cmd.param2) { - case 0: - bind_arg = DSM2_BIND_PULSES; - break; + // PWM_MAIN_MINx + if (!_pwm_min_configured) { + for (unsigned i = 0; i < _max_actuators; i++) { + sprintf(str, "%s_MIN%u", prefix, i + 1); + int32_t pwm_min = -1; - case 1: - bind_arg = DSMX_BIND_PULSES; - break; + if (param_get(param_find(str), &pwm_min) == PX4_OK) { + if (pwm_min >= 0 && pwm_min != 1000) { + _mixing_output.minValue(i) = math::constrain(pwm_min, static_cast(PWM_LOWEST_MIN), + static_cast(PWM_HIGHEST_MIN)); - case 2: - default: - bind_arg = DSMX8_BIND_PULSES; - break; + if (pwm_min != _mixing_output.minValue(i)) { + int32_t pwm_min_new = _mixing_output.minValue(i); + param_set(param_find(str), &pwm_min_new); } - int dsm_ret = dsm_bind_ioctl(bind_arg); - - /* publish ACK */ - if (dsm_ret == OK) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } + } else if (pwm_default_channel_mask & 1 << i) { + _mixing_output.minValue(i) = pwm_min_default; } } + } + + _pwm_min_configured = true; + } - /* - * If parameters have changed, re-send RC mappings to IO - * - * XXX this may be a bit spammy - */ + // PWM_MAIN_MAXx + if (!_pwm_max_configured) { + for (unsigned i = 0; i < _max_actuators; i++) { + sprintf(str, "%s_MAX%u", prefix, i + 1); + int32_t pwm_max = -1; - // check for parameter updates - if (_parameter_update_sub.updated() || _param_update_force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); + if (param_get(param_find(str), &pwm_max) == PX4_OK) { + if (pwm_max >= 0 && pwm_max != 2000) { + _mixing_output.maxValue(i) = math::constrain(pwm_max, static_cast(PWM_LOWEST_MAX), + static_cast(PWM_HIGHEST_MAX)); - _param_update_force = false; + if (pwm_max != _mixing_output.maxValue(i)) { + int32_t pwm_max_new = _mixing_output.maxValue(i); + param_set(param_find(str), &pwm_max_new); + } - if (!_rc_handling_disabled) { - /* re-upload RC input config as it may have changed */ - io_set_rc_config(); + } else if (pwm_default_channel_mask & 1 << i) { + _mixing_output.maxValue(i) = pwm_max_default; } + } + } - /* send RC throttle failsafe value to IO */ - int32_t failsafe_param_val; - param_t failsafe_param = param_find("RC_FAILS_THR"); + _pwm_max_configured = true; + } - if (failsafe_param != PARAM_INVALID) { + // PWM_MAIN_FAILx + if (!_pwm_fail_configured) { + for (unsigned i = 0; i < _max_actuators; i++) { + sprintf(str, "%s_FAIL%u", prefix, i + 1); + int32_t pwm_fail = -1; - param_get(failsafe_param, &failsafe_param_val); + if (param_get(param_find(str), &pwm_fail) == PX4_OK) { + if (pwm_fail >= 0) { + _mixing_output.failsafeValue(i) = math::constrain(pwm_fail, static_cast(0), + static_cast(PWM_HIGHEST_MAX)); - if (failsafe_param_val > 0) { + if (pwm_fail != _mixing_output.failsafeValue(i)) { + int32_t pwm_fail_new = _mixing_output.failsafeValue(i); + param_set(param_find(str), &pwm_fail_new); + } - uint16_t failsafe_thr = failsafe_param_val; - int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + } else { + if (pwm_default_channel_mask & 1 << i) { + _mixing_output.failsafeValue(i) = PWM_MOTOR_OFF; - if (pret != OK) { - mavlink_log_critical(&_mavlink_log_pub, "failsafe upload failed, FS: %d us", (int)failsafe_thr); - } + } else { + _mixing_output.failsafeValue(i) = PWM_SERVO_STOP; } } + } + } - /* Check if the IO safety circuit breaker has been updated */ - bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); - /* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */ - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled); - - /* Check if the flight termination circuit breaker has been updated */ - _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); - /* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */ - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION, !_cb_flighttermination); - - param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan); - param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max); - param_get(param_find("RC_RSSI_PWM_MIN"), &_rssi_pwm_min); - - param_t thermal_param = param_find("SENS_EN_THERMAL"); - - if (thermal_param != PARAM_INVALID) { - - int32_t thermal_p; - param_get(thermal_param, &thermal_p); - - if (thermal_p != _thermal_control || _param_update_force) { - - _thermal_control = thermal_p; - /* set power management state for thermal */ - uint16_t tctrl; - - if (_thermal_control < 0) { - tctrl = PX4IO_THERMAL_IGNORE; - - } else { - tctrl = PX4IO_THERMAL_OFF; - } - - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL, tctrl); - } - } - - float param_val; - param_t parm_handle; - - parm_handle = param_find("TRIM_ROLL"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("TRIM_PITCH"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("TRIM_YAW"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_R_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_ROLL, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_P_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_PITCH, FLOAT_TO_REG(param_val)); - } - - parm_handle = param_find("FW_MAN_Y_SC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SCALE_YAW, FLOAT_TO_REG(param_val)); - } - - /* S.BUS output */ - int sbus_mode; - parm_handle = param_find("PWM_SBUS_MODE"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, &sbus_mode); - - if (sbus_mode == 1) { - /* enable S.BUS 1 */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); - - } else if (sbus_mode == 2) { - /* enable S.BUS 2 */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); - - } else { - /* disable S.BUS */ - (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, - (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); - } - } - - /* thrust to pwm modelling factor */ - parm_handle = param_find("THR_MDL_FAC"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THR_MDL_FAC, FLOAT_TO_REG(param_val)); - } - - /* maximum motor pwm slew rate */ - parm_handle = param_find("MOT_SLEW_MAX"); - - if (parm_handle != PARAM_INVALID) { - param_get(parm_handle, ¶m_val); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_MOTOR_SLEW_MAX, FLOAT_TO_REG(param_val)); - } - - /* air-mode */ - parm_handle = param_find("MC_AIRMODE"); - - if (parm_handle != PARAM_INVALID) { - int32_t param_val_int; - param_get(parm_handle, ¶m_val_int); - (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_AIRMODE, SIGNED_TO_REG(param_val_int)); - } - - update_params(); - } - - } - - perf_end(_perf_update); - } - - unlock(); - -out: - PX4_DEBUG("exiting"); - - /* clean up the alternate device node */ - if (_primary_pwm_device) { - unregister_driver(PWM_OUTPUT0_DEVICE_PATH); - } - - /* tell the dtor that we are exiting */ - _task = -1; - _exit(0); -} - -void PX4IO::update_params() -{ - // skip update when armed - if (_armed) { - return; - } - - int32_t pwm_min_default = PWM_DEFAULT_MIN; - int32_t pwm_max_default = PWM_DEFAULT_MAX; - int32_t pwm_disarmed_default = 0; - int32_t pwm_rate_default = 50; - - const char *prefix = "PWM_MAIN"; - - param_get(param_find("PWM_MAIN_MIN"), &pwm_min_default); - param_get(param_find("PWM_MAIN_MAX"), &pwm_max_default); - param_get(param_find("PWM_MAIN_DISARM"), &pwm_disarmed_default); - param_get(param_find("PWM_MAIN_RATE"), &pwm_rate_default); - - char str[17]; - - - // PWM_MAIN_MINx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_MIN%u", prefix, i + 1); - int32_t pwm_min = -1; - - if (param_get(param_find(str), &pwm_min) == PX4_OK) { - if (pwm_min >= 0) { - pwm.values[i] = math::constrain(pwm_min, PWM_LOWEST_MIN, PWM_HIGHEST_MIN); - - if (pwm_min != pwm.values[i]) { - int32_t pwm_min_new = pwm.values[i]; - param_set(param_find(str), &pwm_min_new); - } - - } else { - pwm.values[i] = pwm_min_default; - } - } - } - - io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm.values, pwm.channel_count); - } - - // PWM_MAIN_MAXx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_MAX%u", prefix, i + 1); - int32_t pwm_max = -1; - - if (param_get(param_find(str), &pwm_max) == PX4_OK) { - if (pwm_max >= 0) { - pwm.values[i] = math::constrain(pwm_max, PWM_LOWEST_MAX, PWM_HIGHEST_MAX); - - if (pwm_max != pwm.values[i]) { - int32_t pwm_max_new = pwm.values[i]; - param_set(param_find(str), &pwm_max_new); - } - - } else { - pwm.values[i] = pwm_max_default; - } - } - } - - io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm.values, pwm.channel_count); - } - - // PWM_MAIN_FAILx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - - for (unsigned i = 0; i < _max_actuators; i++) { - sprintf(str, "%s_FAIL%u", prefix, i + 1); - int32_t pwm_fail = -1; - - if (param_get(param_find(str), &pwm_fail) == PX4_OK) { - if (pwm_fail >= 0) { - pwm.values[i] = math::constrain(pwm_fail, 0, PWM_HIGHEST_MAX); - - if (pwm_fail != pwm.values[i]) { - int32_t pwm_fail_new = pwm.values[i]; - param_set(param_find(str), &pwm_fail_new); - } - } - } - } - - io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm.values, pwm.channel_count); - } + _pwm_fail_configured = true; + updateFailsafe(); + } // PWM_MAIN_DISx - { - pwm_output_values pwm{}; - pwm.channel_count = _max_actuators; - + if (!_pwm_dis_configured) { for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_DIS%u", prefix, i + 1); int32_t pwm_dis = -1; if (param_get(param_find(str), &pwm_dis) == PX4_OK) { - if (pwm_dis >= 0) { - pwm.values[i] = math::constrain(pwm_dis, 0, PWM_HIGHEST_MAX); + if (pwm_dis >= 0 && pwm_dis != 900) { + _mixing_output.disarmedValue(i) = math::constrain(pwm_dis, static_cast(0), + static_cast(PWM_HIGHEST_MAX)); - if (pwm_dis != pwm.values[i]) { - int32_t pwm_dis_new = pwm.values[i]; + if (pwm_dis != _mixing_output.disarmedValue(i)) { + int32_t pwm_dis_new = _mixing_output.disarmedValue(i); param_set(param_find(str), &pwm_dis_new); } - } else { - pwm.values[i] = pwm_disarmed_default; + } else if (pwm_default_channel_mask & 1 << i) { + _mixing_output.disarmedValue(i) = pwm_disarmed_default; } } } - io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm.values, pwm.channel_count); + _pwm_dis_configured = true; + updateDisarmed(); } // PWM_MAIN_REVx - { - int16_t reverse_pwm_mask = 0; + if (!_pwm_rev_configured) { + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + reverse_pwm_mask = 0; for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_REV%u", prefix, i + 1); @@ -1335,111 +862,30 @@ void PX4IO::update_params() } } - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, reverse_pwm_mask); + _pwm_rev_configured = true; } // PWM_MAIN_TRIMx { - uint16_t values[8] {}; + int16_t values[8] {}; for (unsigned i = 0; i < _max_actuators; i++) { sprintf(str, "%s_TRIM%u", prefix, i + 1); float pwm_trim = 0.f; if (param_get(param_find(str), &pwm_trim) == PX4_OK) { - values[i] = (int16_t)(10000 * pwm_trim); + values[i] = roundf(10000 * pwm_trim); } } - // copy the trim values to the mixer offsets - io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, values, _max_actuators); - } -} - -int -PX4IO::io_set_control_groups() -{ - int ret = io_set_control_state(0); - - /* send auxiliary control groups */ - (void)io_set_control_state(1); - (void)io_set_control_state(2); - (void)io_set_control_state(3); - - return ret; -} - -int -PX4IO::io_set_control_state(unsigned group) -{ - actuator_controls_s controls{}; ///< actuator outputs - - /* get controls */ - bool changed = false; - - switch (group) { - case 0: { - orb_check(_t_actuator_controls_0, &changed); - - if (changed) { - orb_copy(ORB_ID(actuator_controls_0), _t_actuator_controls_0, &controls); - perf_set_elapsed(_perf_sample_latency, hrt_elapsed_time(&controls.timestamp_sample)); - } + if (_mixing_output.mixers()) { + // copy the trim values to the mixer offsets + _mixing_output.mixers()->set_trims(values, _max_actuators); } - break; - - case 1: - changed = _t_actuator_controls_1.update(&controls); - break; - - case 2: - changed = _t_actuator_controls_2.update(&controls); - break; - - case 3: - changed = _t_actuator_controls_3.update(&controls); - break; - - } - - if (!changed && (!_in_esc_calibration_mode || group != 0)) { - return -1; - - } else if (_in_esc_calibration_mode && group == 0) { - /* modify controls to get max pwm (full thrust) on every esc */ - memset(&controls, 0, sizeof(controls)); - - /* set maximum thrust */ - controls.control[3] = 1.0f; - } - - uint16_t regs[sizeof(controls.control) / sizeof(controls.control[0])] = {}; - - for (unsigned i = 0; (i < _max_controls) && (i < sizeof(controls.control) / sizeof(controls.control[0])); i++) { - /* ensure FLOAT_TO_REG does not produce an integer overflow */ - const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f); - - if (!isfinite(ctrl)) { - regs[i] = INT16_MAX; - - } else { - regs[i] = FLOAT_TO_REG(ctrl); - } - - } - - if (!_test_fmu_fail && !_motor_test.in_test_mode) { - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, math::min(_max_controls, - sizeof(controls.control) / sizeof(controls.control[0]))); - - } else { - return OK; } } -void -PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) +void PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) { /* publish ACK */ uORB::Publication vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; @@ -1452,74 +898,6 @@ PX4IO::answer_command(const vehicle_command_s &cmd, uint8_t result) vehicle_command_ack_pub.publish(command_ack); } -void -PX4IO::handle_motor_test() -{ - test_motor_s test_motor; - - while (_motor_test.test_motor_sub.update(&test_motor)) { - if (test_motor.driver_instance != 0 || - hrt_elapsed_time(&test_motor.timestamp) > 100_ms) { - continue; - } - - bool in_test_mode = test_motor.action == test_motor_s::ACTION_RUN; - - if (in_test_mode != _motor_test.in_test_mode) { - // reset all outputs to disarmed on state change - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - for (unsigned i = 0; i < _max_actuators; ++i) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]); - } - } - } - - if (in_test_mode) { - unsigned idx = test_motor.motor_number; - - if (idx < _max_actuators) { - if (test_motor.value < 0.f) { - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, pwm_disarmed.values[idx]); - } - - } else { - pwm_output_values pwm_min; - pwm_output_values pwm_max; - - if (io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm_min.values, _max_actuators) == 0 && - io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm_max.values, _max_actuators) == 0) { - - uint16_t value = math::constrain(pwm_min.values[idx] + - (uint16_t)((pwm_max.values[idx] - pwm_min.values[idx]) * test_motor.value), - pwm_min.values[idx], pwm_max.values[idx]); - io_reg_set(PX4IO_PAGE_DIRECT_PWM, idx, value); - } - } - } - - if (test_motor.timeout_ms > 0) { - _motor_test.timeout = test_motor.timestamp + test_motor.timeout_ms * 1000; - - } else { - _motor_test.timeout = 0; - } - } - - _motor_test.in_test_mode = in_test_mode; - } - - // check for timeouts - if (_motor_test.timeout != 0 && hrt_absolute_time() > _motor_test.timeout) { - _motor_test.in_test_mode = false; - _motor_test.timeout = 0; - } -} - int PX4IO::io_set_arming_state() { @@ -1529,17 +907,13 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; if (_t_actuator_armed.copy(&armed)) { - _in_esc_calibration_mode = armed.in_esc_calibration_mode; - - if (armed.armed || _in_esc_calibration_mode) { + if (armed.armed || armed.in_esc_calibration_mode) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - _armed = armed.armed; - if (armed.prearmed) { set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; @@ -1579,19 +953,6 @@ PX4IO::io_set_arming_state() } } - vehicle_control_mode_s control_mode; - - if (_t_vehicle_control_mode.copy(&control_mode)) { - if (control_mode.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - _override_available = true; - - } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - _override_available = false; - } - } - if (_last_written_arming_s != set || _last_written_arming_c != clear) { _last_written_arming_s = set; _last_written_arming_c = clear; @@ -1601,181 +962,7 @@ PX4IO::io_set_arming_state() return 0; } -int -PX4IO::disable_rc_handling() -{ - _rc_handling_disabled = true; - return io_disable_rc_handling(); -} - -int -PX4IO::io_disable_rc_handling() -{ - uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; - uint16_t clear = 0; - - return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); -} - -int -PX4IO::io_set_rc_config() -{ - unsigned offset = 0; - int input_map[_max_rc_input]; - int32_t ichan; - int ret = OK; - - /* - * Generate the input channel -> control channel mapping table; - * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical - * controls. - */ - - /* fill the mapping with an error condition triggering value */ - for (unsigned i = 0; i < _max_rc_input; i++) { - input_map[i] = UINT8_MAX; - } - - /* - * NOTE: The indices for mapped channels are 1-based - * for compatibility reasons with existing - * autopilots / GCS'. - */ - - /* ROLL */ - param_get(param_find("RC_MAP_ROLL"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 0; - } - - /* PITCH */ - param_get(param_find("RC_MAP_PITCH"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 1; - } - - /* YAW */ - param_get(param_find("RC_MAP_YAW"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 2; - } - - /* THROTTLE */ - param_get(param_find("RC_MAP_THROTTLE"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 3; - } - - /* FLAPS */ - param_get(param_find("RC_MAP_FLAPS"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 4; - } - - /* AUX 1*/ - param_get(param_find("RC_MAP_AUX1"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 5; - } - - /* AUX 2*/ - param_get(param_find("RC_MAP_AUX2"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 6; - } - - /* AUX 3*/ - param_get(param_find("RC_MAP_AUX3"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - input_map[ichan - 1] = 7; - } - - /* MAIN MODE SWITCH */ - param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { - /* use out of normal bounds index to indicate special channel */ - input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; - } - - /* - * Iterate all possible RC inputs. - */ - for (unsigned i = 0; i < _max_rc_input; i++) { - uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; - char pname[16]; - float fval; - - /* - * RC params are floats, but do only - * contain integer values. Do not scale - * or cast them, let the auto-typeconversion - * do its job here. - * Channels: 500 - 2500 - * Inverted flag: -1 (inverted) or 1 (normal) - */ - - sprintf(pname, "RC%u_MIN", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MIN] = fval; - - sprintf(pname, "RC%u_TRIM", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_CENTER] = fval; - - sprintf(pname, "RC%u_MAX", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_MAX] = fval; - - sprintf(pname, "RC%u_DZ", i + 1); - param_get(param_find(pname), &fval); - regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; - - regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; - - regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - sprintf(pname, "RC%u_REV", i + 1); - param_get(param_find(pname), &fval); - - /* - * This has been taken for the sake of compatibility - * with APM's setup / mission planner: normal: 1, - * inverted: -1 - */ - if (fval < 0) { - regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; - } - - /* send channel config to IO */ - ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); - - if (ret != OK) { - PX4_ERR("rc config upload failed"); - break; - } - - /* check the IO initialisation flag */ - if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { - mavlink_log_critical(&_mavlink_log_pub, "config for RC%u rejected by IO", i + 1); - break; - } - - offset += PX4IO_P_RC_CONFIG_STRIDE; - } - - return ret; -} - -int -PX4IO::io_handle_status(uint16_t status) +int PX4IO::io_handle_status(uint16_t status) { int ret = 1; /** @@ -1783,15 +970,12 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, - PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { @@ -1808,31 +992,31 @@ PX4IO::io_handle_status(uint16_t status) } /** - * Get and handle the safety status + * Get and handle the safety button status */ - const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - const bool override_enabled = status & PX4IO_P_STATUS_FLAGS_OVERRIDE; + const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; - // publish immediately on change, otherwise at 1 Hz - if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) - || (_safety.safety_off != safety_off) - || (_safety.override_available != _override_available) - || (_safety.override_enabled != override_enabled)) { + if (safety_button_pressed) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0); + _button_publisher.safetyButtonTriggerEvent(); + } - _safety.safety_switch_available = true; - _safety.safety_off = safety_off; - _safety.override_available = _override_available; - _safety.override_enabled = override_enabled; - _safety.timestamp = hrt_absolute_time(); + /** + * Inform PX4IO board about safety_off state for LED control + */ + vehicle_status_s vehicle_status; - _to_safety.publish(_safety); + if (_t_vehicle_status.update(&vehicle_status)) { + if (_previous_safety_off != vehicle_status.safety_off) { + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off); + _previous_safety_off = vehicle_status.safety_off; + } } return ret; } -int -PX4IO::dsm_bind_ioctl(int dsmMode) +int PX4IO::dsm_bind_ioctl(int dsmMode) { // Do not bind if invalid pulses are provided if (dsmMode != DSM2_BIND_PULSES && @@ -1850,37 +1034,17 @@ PX4IO::dsm_bind_ioctl(int dsmMode) return -EINVAL; } - // Check if safety was off - bool safety_off = (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF); - int ret = -1; - - // Put safety on - if (safety_off) { - // re-enable safety - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0); - - // set new status - _status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF); - } - PX4_INFO("Binding DSM%s RX", (dsmMode == DSM2_BIND_PULSES) ? "2" : ((dsmMode == DSMX_BIND_PULSES) ? "-X" : "-X8")); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + int ret = OK; + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); px4_usleep(500000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); px4_usleep(72000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4)); + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4)); px4_usleep(50000); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); - ret = OK; - - // Put safety back off - if (safety_off) { - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, - PX4IO_P_STATUS_FLAGS_SAFETY_OFF); - _status |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - } + ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); if (ret != OK) { PX4_INFO("Binding DSM failed"); @@ -1889,8 +1053,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode) return ret; } -int -PX4IO::io_get_status() +int PX4IO::io_get_status() { /* get * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, @@ -1939,50 +1102,63 @@ PX4IO::io_get_status() // PX4IO_P_STATUS_FLAGS status.status_outputs_armed = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; - status.status_override = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_OVERRIDE; status.status_rc_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_OK; status.status_rc_ppm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_PPM; status.status_rc_dsm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_DSM; status.status_rc_sbus = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SBUS; + status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24; + status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD; + status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; status.status_fmu_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_OK; status.status_raw_pwm = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RAW_PWM; - status.status_mixer_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_MIXER_OK; status.status_arm_sync = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC; status.status_init_ok = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK; status.status_failsafe = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE; - status.status_safety_off = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF; - status.status_fmu_initialized = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; - status.status_rc_st24 = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_ST24; - status.status_rc_sumd = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_RC_SUMD; + status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT; // PX4IO_P_STATUS_ALARMS - status.alarm_vbatt_low = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_VBATT_LOW; - status.alarm_temperature = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_TEMPERATURE; - status.alarm_servo_current = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT; - status.alarm_acc_current = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_ACC_CURRENT; - status.alarm_fmu_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_FMU_LOST; status.alarm_rc_lost = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST; status.alarm_pwm_error = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_PWM_ERROR; - status.alarm_vservo_fault = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT; // PX4IO_P_SETUP_ARMING status.arming_io_arm_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_IO_ARM_OK; status.arming_fmu_armed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_ARMED; status.arming_fmu_prearmed = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FMU_PREARMED; - status.arming_manual_override_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; status.arming_failsafe_custom = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; - status.arming_inair_restart_ok = SETUP_ARMING & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK; - status.arming_always_pwm_enable = SETUP_ARMING & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - status.arming_rc_handling_disabled = SETUP_ARMING & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED; status.arming_lockdown = SETUP_ARMING & PX4IO_P_SETUP_ARMING_LOCKDOWN; status.arming_force_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; status.arming_termination_failsafe = SETUP_ARMING & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - status.arming_override_immediate = SETUP_ARMING & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE; - for (unsigned i = 0; i < _max_actuators; i++) { - status.actuators[i] = static_cast(io_reg_get(PX4IO_PAGE_ACTUATORS, i)); - status.servos[i] = io_reg_get(PX4IO_PAGE_SERVOS, i); + status.pwm[i] = io_reg_get(PX4IO_PAGE_SERVOS, i); + status.pwm_disarmed[i] = io_reg_get(PX4IO_PAGE_DISARMED_PWM, i); + status.pwm_failsafe[i] = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i); + } + + if (_mixing_output.useDynamicMixing()) { + + int i = 0; + + for (uint8_t offset = PX4IO_P_SETUP_PWM_RATE_GROUP0; offset <= PX4IO_P_SETUP_PWM_RATE_GROUP3; ++offset) { + // This is a bit different than below, setting the groups, not the channels + status.pwm_rate_hz[i++] = io_reg_get(PX4IO_PAGE_SETUP, offset); + } + + } else { + // PWM rates, 0 = low rate, 1 = high rate + const uint16_t pwm_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + + const int pwm_low_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + const int pwm_high_rate = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + + for (unsigned i = 0; i < _max_actuators; i++) { + if (pwm_rate & (1 << i)) { + status.pwm_rate_hz[i] = pwm_high_rate; + + } else { + status.pwm_rate_hz[i] = pwm_low_rate; + } + } } uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); @@ -2003,11 +1179,21 @@ PX4IO::io_get_status() return ret; } -int -PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) -{ - uint32_t channel_count; - int ret; +int PX4IO::io_publish_raw_rc() +{ + const uint16_t rc_valid_update_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_FRAME_COUNT); + const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count); + _rc_valid_update_count = rc_valid_update_count; + + if (!rc_updated) { + return 0; + } + + input_rc_s input_rc{}; + input_rc.timestamp_last_signal = hrt_absolute_time(); + + /* set the RC status flag ORDER MATTERS! */ + input_rc.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; @@ -2020,7 +1206,7 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + int ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) { return ret; @@ -2030,15 +1216,13 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[PX4IO_P_RAW_RC_COUNT]; + uint32_t channel_count = regs[PX4IO_P_RAW_RC_COUNT]; /* limit the channel count */ if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; } - _rc_chan_count = channel_count; - input_rc.timestamp = hrt_absolute_time(); input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; @@ -2066,12 +1250,6 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; input_rc.channel_count = channel_count; - /* rc_lost has to be set before the call to this function */ - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { - _rc_last_valid = input_rc.timestamp; - } - - input_rc.timestamp_last_signal = _rc_last_valid; /* FIELDS NOT SET HERE */ /* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */ @@ -2095,109 +1273,39 @@ PX4IO::io_get_raw_rc_input(input_rc_s &input_rc) } /* get RSSI from input channel */ - if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) { - int rssi = ((input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) * 100) / - (_rssi_pwm_max - _rssi_pwm_min); - rssi = rssi > 100 ? 100 : rssi; - rssi = rssi < 0 ? 0 : rssi; - input_rc.rssi = rssi; - } - - return ret; -} - -int -PX4IO::io_publish_raw_rc() -{ - - /* fetch values from IO */ - input_rc_s rc_val; - - /* set the RC status flag ORDER MATTERS! */ - rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); - - int ret = io_get_raw_rc_input(rc_val); + if (_param_rc_rssi_pwm_chan.get() > 0 && _param_rc_rssi_pwm_chan.get() <= input_rc_s::RC_INPUT_MAX_CHANNELS) { + const auto &min = _param_rc_rssi_pwm_min.get(); + const auto &max = _param_rc_rssi_pwm_max.get(); - if (ret != OK) { - return ret; + if (max - min != 0) { + int rssi = ((input_rc.values[_param_rc_rssi_pwm_chan.get() - 1] - min) * 100) / (max - min); + input_rc.rssi = math::constrain(rssi, 0, 100); + } } /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; } else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; - - } else { - rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN; - - /* only keep publishing RC input if we ever got a valid input */ - if (_rc_last_valid == 0) { - /* we have never seen valid RC signals, abort */ - return OK; - } - } - - _to_input_rc.publish(rc_val); - - return OK; -} - -int -PX4IO::io_publish_pwm_outputs() -{ - if (_hitl_mode) { - return OK; - } - - /* get servo values from IO */ - uint16_t ctl[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); - - if (ret != OK) { - return ret; - } - - actuator_outputs_s outputs = {}; - outputs.timestamp = hrt_absolute_time(); - outputs.noutputs = _max_actuators; - - /* convert from register format to float */ - for (unsigned i = 0; i < _max_actuators; i++) { - outputs.output[i] = ctl[i]; - } - - _to_outputs.publish(outputs); - - /* get mixer status flags from IO */ - MultirotorMixer::saturation_status saturation_status; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &saturation_status.value, 1); - - if (ret != OK) { - return ret; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24; } - /* publish mixer status */ - if (saturation_status.flags.valid) { - multirotor_motor_limits_s motor_limits{}; - motor_limits.timestamp = hrt_absolute_time(); - motor_limits.saturation_status = saturation_status.value; + if (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN) { - _to_mixer_status.publish(motor_limits); + _to_input_rc.publish(input_rc); } - return OK; + return ret; } -int -PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +int PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { @@ -2205,24 +1313,24 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned return -EINVAL; } - int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + perf_begin(_interface_write_perf); + int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + perf_end(_interface_write_perf); if (ret != (int)num_values) { - PX4_DEBUG("io_reg_set(%hhu,%hhu,%u): error %d", page, offset, num_values, ret); + PX4_DEBUG("io_reg_set(%" PRIu8 ",%" PRIu8 ",%u): error %d", page, offset, num_values, ret); return -1; } return OK; } -int -PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) +int PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) { return io_reg_set(page, offset, &value, 1); } -int -PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) { /* range check the transfer */ if (num_values > ((_max_transfer) / sizeof(*values))) { @@ -2230,18 +1338,19 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v return -EINVAL; } + perf_begin(_interface_read_perf); int ret = _interface->read((page << 8) | offset, reinterpret_cast(values), num_values); + perf_end(_interface_read_perf); if (ret != (int)num_values) { - PX4_DEBUG("io_reg_get(%hhu,%hhu,%u): data error %d", page, offset, num_values, ret); + PX4_DEBUG("io_reg_get(%" PRIu8 ",%" PRIu8 ",%u): data error %d", page, offset, num_values, ret); return -1; } return OK; } -uint32_t -PX4IO::io_reg_get(uint8_t page, uint8_t offset) +uint32_t PX4IO::io_reg_get(uint8_t page, uint8_t offset) { uint16_t value; @@ -2252,13 +1361,10 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset) return value; } -int -PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) +int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) { - int ret; - uint16_t value; - - ret = io_reg_get(page, offset, &value, 1); + uint16_t value = 0; + int ret = io_reg_get(page, offset, &value, 1); if (ret != OK) { return ret; @@ -2270,246 +1376,40 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t return io_reg_set(page, offset, value); } -int -PX4IO::print_debug() -{ -#if defined(CONFIG_ARCH_BOARD_PX4_FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4_FMU_V3) - int io_fd = -1; - - if (io_fd <= 0) { - io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); - } - - /* read IO's output */ - if (io_fd >= 0) { - pollfd fds[1]; - fds[0].fd = io_fd; - fds[0].events = POLLIN; - - px4_usleep(500); - int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); - - if (pret > 0) { - int count; - char buf[65]; - - do { - count = ::read(io_fd, buf, sizeof(buf) - 1); - - if (count > 0) { - /* enforce null termination */ - buf[count] = '\0'; - warnx("IO CONSOLE: %s", buf); - } - - } while (count > 0); - } - - ::close(io_fd); - return 0; - } - -#endif - return 1; - -} - -int -PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) -{ - /* get debug level */ - int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); - - uint8_t frame[_max_transfer]; - - do { - - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; - - do { - unsigned count = buflen; - - if (count > max_len) { - count = max_len; - } - - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - - } else { - continue; - } - - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - - if (total_len % 2) { - msg->text[count] = '\0'; - total_len++; - } - - int ret; - - for (int i = 0; i < 30; i++) { - /* failed, but give it a 2nd shot */ - ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); - - if (ret) { - px4_usleep(333); - - } else { - break; - } - } - - /* print mixer chunk */ - if (debuglevel > 5 || ret) { - - warnx("fmu sent: \"%s\"", msg->text); - - /* read IO's output */ - print_debug(); - } - - if (ret) { - PX4_ERR("mixer send error %d", ret); - return ret; - } - - msg->action = F2I_MIXER_ACTION_APPEND; - - } while (buflen > 0); - - int ret; - - /* send the closing newline */ - msg->text[0] = '\n'; - msg->text[1] = '\0'; - - for (int i = 0; i < 30; i++) { - /* failed, but give it a 2nd shot */ - ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); - - if (ret) { - px4_usleep(333); - - } else { - break; - } - } - - if (ret == 0) { - /* success, exit */ - break; - } - - retries--; - - } while (retries > 0); - - if (retries == 0) { - mavlink_log_info(&_mavlink_log_pub, "[IO] mixer upload fail"); - /* load must have failed for some reason */ - return -EINVAL; - - } else { - /* all went well, set the mixer ok flag */ - return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK); - } -} - -void -PX4IO::print_status(bool extended_status) +int PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", + printf("protocol %" PRIu32 " hardware %" PRIu32 " bootloader %" PRIu32 " buffer %" PRIu32 "B crc 0x%04" PRIu32 "%04" + PRIu32 "\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC + 1)); - printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + + printf("%" PRIu32 " controls %" PRIu32 " actuators %" PRIu32 " R/C inputs %" PRIu32 " analog inputs\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT)); /* status */ uORB::SubscriptionData status_sub{ORB_ID(px4io_status)}; status_sub.update(); - print_message(status_sub.get()); + print_message(ORB_ID(px4io_status), status_sub.get()); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0x0000); - uint16_t pwm_invert_mask = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE); - printf("\n"); - printf("reversed outputs: ["); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_"); - } - - printf("]"); - - float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL)); - float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH)); - float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW)); - - printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n", - (double)trim_roll, (double)trim_pitch, (double)trim_yaw); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - printf("%hu raw R/C inputs", raw_inputs); + printf("%" PRIu16 " raw R/C inputs", raw_inputs); for (unsigned i = 0; i < raw_inputs; i++) { - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); - } - - printf("\n"); - - uint16_t io_status_flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - uint16_t flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("R/C flags: 0x%04hx%s%s%s%s%s\n", flags, - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") - ); - - if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); - printf("RC data (PPM frame len) %d us\n", frame_len); - - if ((frame_len - raw_inputs * 2000 - 3000) < 0) { - printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); - } - } - - uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); - printf("mapped R/C inputs 0x%04hx", mapped_inputs); - - for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs & (1 << i)) { - printf(" %u:%hd", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); - } + printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); } printf("\n"); @@ -2517,65 +1417,22 @@ PX4IO::print_status(bool extended_status) printf("ADC inputs"); for (unsigned i = 0; i < adc_inputs; i++) { - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf(" %" PRIu32, io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); } printf("\n"); /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04hx%s%s%s%s\n", features, + printf("features 0x%04" PRIx16 "%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), - ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); - printf("rates 0x%04x default %u alt %u sbus %u\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); - printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - - for (unsigned group = 0; group < 4; group++) { - printf("controls %u:", group); - - for (unsigned i = 0; i < _max_controls; i++) { - printf(" %hd", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); - } - - printf("\n"); - } - - if (extended_status) { - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04hx%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); - } - } - - printf("failsafe"); - - for (unsigned i = 0; i < _max_actuators; i++) { - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - } - - printf("\ndisarmed values"); + printf("sbus rate %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SBUS_RATE)); - for (unsigned i = 0; i < _max_actuators; i++) { - printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); - } + printf("debuglevel %" PRIu32 "\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); /* IMU heater (Pixhawk 2.1) */ uint16_t heater_level = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_THERMAL); @@ -2589,15 +1446,13 @@ PX4IO::print_status(bool extended_status) } } - if (_hitl_mode) { - printf("\nHITL Mode"); - } - printf("\n"); + + _mixing_output.printStatus(); + return 0; } -int -PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { SmartLock lock_guard(_lock); int ret = OK; @@ -2605,41 +1460,60 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) /* regular ioctl? */ switch (cmd) { case PWM_SERVO_ARM: + PX4_DEBUG("PWM_SERVO_ARM"); /* set the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED); break; case PWM_SERVO_SET_ARM_OK: + PX4_DEBUG("PWM_SERVO_SET_ARM_OK"); /* set the 'OK to arm' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK); break; case PWM_SERVO_CLEAR_ARM_OK: + PX4_DEBUG("PWM_SERVO_CLEAR_ARM_OK"); /* clear the 'OK to arm' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0); break; case PWM_SERVO_DISARM: + PX4_DEBUG("PWM_SERVO_DISARM"); /* clear the 'armed' bit */ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); /* get the default update rate */ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); break; case PWM_SERVO_SET_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_SET_UPDATE_RATE"); + + if (_mixing_output.useDynamicMixing()) { + ret = -EINVAL; + break; + } + /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; case PWM_SERVO_GET_UPDATE_RATE: + PX4_DEBUG("PWM_SERVO_GET_UPDATE_RATE"); /* get the alternative update rate */ *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); break; case PWM_SERVO_SET_SELECT_UPDATE_RATE: { + PX4_DEBUG("PWM_SERVO_SET_SELECT_UPDATE_RATE"); + + if (_mixing_output.useDynamicMixing()) { + ret = -EINVAL; + break; + } /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); @@ -2660,195 +1534,95 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } case PWM_SERVO_GET_SELECT_UPDATE_RATE: - + PX4_DEBUG("PWM_SERVO_GET_SELECT_UPDATE_RATE"); *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); break; - case PWM_SERVO_SET_FAILSAFE_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) - /* fail with error */ - { - return -E2BIG; - } - - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); - break; - } - case PWM_SERVO_GET_FAILSAFE_PWM: { + PX4_DEBUG("PWM_SERVO_GET_FAILSAFE_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; - } - - break; - } - - case PWM_SERVO_SET_DISARMED_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) - /* fail with error */ - { - return -E2BIG; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.failsafeValue(i); } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); break; } case PWM_SERVO_GET_DISARMED_PWM: { + PX4_DEBUG("PWM_SERVO_GET_DISARMED_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.disarmedValue(i); } break; } case PWM_SERVO_SET_MIN_PWM: { + PX4_DEBUG("PWM_SERVO_SET_MIN_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (pwm->channel_count > _max_actuators) + if (pwm->channel_count > _max_actuators) { /* fail with error */ - { return -E2BIG; } - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { + _mixing_output.minValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MIN, (uint16_t)PWM_HIGHEST_MIN); + } + } + break; } case PWM_SERVO_GET_MIN_PWM: { + PX4_DEBUG("PWM_SERVO_GET_MIN_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; pwm->channel_count = _max_actuators; - ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.minValue(i); } break; } case PWM_SERVO_SET_MAX_PWM: { + PX4_DEBUG("PWM_SERVO_SET_MAX_PWM"); struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - { - return -E2BIG; - } - - /* copy values to registers in IO */ - ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); - break; - } - - case PWM_SERVO_GET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; - } - } - - break; - - case PWM_SERVO_GET_TRIM_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - pwm->channel_count = _max_actuators; - - ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm->values, _max_actuators); - - if (ret != OK) { - ret = -EIO; - } - } - - break; - - case PWM_SERVO_GET_COUNT: - *(unsigned *)arg = _max_actuators; - break; - - case PWM_SERVO_SET_DISABLE_LOCKDOWN: - _lockdown_override = arg; - break; - - case PWM_SERVO_GET_DISABLE_LOCKDOWN: - *(unsigned *)arg = _lockdown_override; - break; - - case PWM_SERVO_SET_FORCE_SAFETY_OFF: - /* force safety swith off */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); - break; - - case PWM_SERVO_SET_FORCE_SAFETY_ON: - /* force safety switch on */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); - break; - - case PWM_SERVO_SET_FORCE_FAILSAFE: - - /* force failsafe mode instantly */ - if (arg == 0) { - /* clear force failsafe flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0); - - } else { - /* set force failsafe flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); - } - - break; - - case PWM_SERVO_SET_TERMINATION_FAILSAFE: - - /* if failsafe occurs, do not allow the system to recover */ - if (arg == 0) { - /* clear termination failsafe flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + if (pwm->channel_count > _max_actuators) { + /* fail with error */ + return -E2BIG; + } - } else { - /* set termination failsafe flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] != 0 && !_mixing_output.useDynamicMixing()) { + _mixing_output.maxValue(i) = math::constrain(pwm->values[i], (uint16_t)PWM_LOWEST_MAX, (uint16_t)PWM_HIGHEST_MAX); + } + } } - break; - case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: - - /* control whether override on FMU failure is - immediate or waits for override threshold on mode - switch */ - if (arg == 0) { - /* clear override immediate flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + case PWM_SERVO_GET_MAX_PWM: { + PX4_DEBUG("PWM_SERVO_GET_MAX_PWM"); + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + pwm->channel_count = _max_actuators; - } else { - /* set override immediate flag */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _mixing_output.maxValue(i); + } } + break; + case PWM_SERVO_GET_COUNT: + PX4_DEBUG("PWM_SERVO_GET_COUNT"); + *(unsigned *)arg = _max_actuators; break; case DSM_BIND_START: @@ -2856,32 +1630,6 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = dsm_bind_ioctl(arg); break; - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { - - /* TODO: we could go lower for e.g. TurboPWM */ - unsigned channel = cmd - PWM_SERVO_SET(0); - - /* PWM needs to be either 0 or in the valid range. */ - if ((arg != 0) && ((channel >= _max_actuators) || - (arg < PWM_LOWEST_MIN) || - (arg > PWM_HIGHEST_MAX))) { - ret = -EINVAL; - - } else { - if (!_test_fmu_fail && _motor_test.in_test_mode) { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - - } else { - /* Just silently accept the ioctl without doing anything - * in test mode. */ - ret = OK; - } - } - - break; - } - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { unsigned channel = cmd - PWM_SERVO_GET(0); @@ -2917,33 +1665,24 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_SET_MODE: { - // reset all channels to disarmed when entering/leaving test mode, so that we don't - // accidentially use values from previous tests - pwm_output_values pwm_disarmed; - - if (io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, pwm_disarmed.values, _max_actuators) == 0) { - for (unsigned i = 0; i < _max_actuators; ++i) { - io_reg_set(PX4IO_PAGE_DIRECT_PWM, i, pwm_disarmed.values[i]); - } - } - - _motor_test.in_test_mode = (arg == PWM_SERVO_ENTER_TEST_MODE); - ret = (arg == PWM_SERVO_ENTER_TEST_MODE || PWM_SERVO_EXIT_TEST_MODE) ? 0 : -EINVAL; - } - break; - case MIXERIOCRESET: - ret = 0; /* load always resets */ + PX4_DEBUG("MIXERIOCRESET"); + _mixing_output.resetMixer(); break; case MIXERIOCLOADBUF: { + PX4_DEBUG("MIXERIOCLOADBUF"); + const char *buf = (const char *)arg; - ret = mixer_send(buf, strlen(buf)); + unsigned buflen = strlen(buf); + ret = _mixing_output.loadMixer(buf, buflen); + break; } case PX4IO_SET_DEBUG: + PX4_DEBUG("PX4IO_SET_DEBUG"); + /* set the debug level */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; @@ -2955,27 +1694,24 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } - // re-enable safety - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); - - if (ret != PX4_OK) { - PX4_ERR("IO refused to re-enable safety"); - } - - // set new status - _status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + /* For Legacy PX4IO Firmware only: + * If IO has already safety off it won't accept going into bootloader mode, + * therefore we need to set safety on first. */ + io_reg_set(PX4IO_PAGE_SETUP, 14, 22027); /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ usleep(1); ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); if (ret != PX4_OK) { - PX4_ERR("IO refused to reboot"); + PX4_WARN("IO refused to reboot"); } break; case PX4IO_CHECK_CRC: { + PX4_DEBUG("PX4IO_CHECK_CRC"); + /* check IO firmware CRC against passed value */ uint32_t io_crc = 0; ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); @@ -2985,48 +1721,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } if (io_crc != arg) { - PX4_DEBUG("Firmware CRC mismatch 0x%08x 0x%08lx", io_crc, arg); + PX4_DEBUG("Firmware CRC mismatch 0x%08" PRIx32 " 0x%08lx", io_crc, arg); return -EINVAL; } break; } - case PX4IO_INAIR_RESTART_ENABLE: - - /* set/clear the 'in-air restart' bit */ - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); - } - - break; - - case RC_INPUT_ENABLE_RSSI_ANALOG: - - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); - } - - break; - - case RC_INPUT_ENABLE_RSSI_PWM: - - if (arg) { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); - - } else { - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); - } - - break; - case SBUS_SET_PROTO_VERSION: + PX4_DEBUG("SBUS_SET_PROTO_VERSION"); if (arg == 1) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); @@ -3064,145 +1767,52 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } -extern "C" __EXPORT int px4io_main(int argc, char *argv[]); - -namespace -{ - -device::Device * -get_interface() +static device::Device *get_interface() { - device::Device *interface = nullptr; - -#ifdef PX4IO_SERIAL_BASE - interface = PX4IO_serial_interface(); -#endif + device::Device *interface = PX4IO_serial_interface(); if (interface != nullptr) { - goto got; - } - - errx(1, "cannot alloc interface"); - -got: - - if (interface->init() != OK) { - delete interface; - errx(1, "interface init failed"); - } - - return interface; -} - -void -start(int argc, char *argv[]) -{ - if (g_dev != nullptr) { - errx(0, "already loaded"); - } - - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - delete interface; - errx(1, "driver allocation failed"); - } - - bool rc_handling_disabled = false; - bool hitl_mode = false; - - /* disable RC handling and/or actuator_output publication on request */ - for (int extra_args = 1; extra_args < argc; extra_args++) { - if (!strcmp(argv[extra_args], "norc")) { - rc_handling_disabled = true; - - } else if (!strcmp(argv[extra_args], "hil")) { - hitl_mode = true; - - } else if (argv[extra_args][0] != '\0') { - PX4_WARN("unknown argument: %s", argv[extra_args]); + if (interface->init() != OK) { + PX4_ERR("interface init failed"); + delete interface; + interface = nullptr; } } - if (OK != g_dev->init(rc_handling_disabled, hitl_mode)) { - delete g_dev; - g_dev = nullptr; - errx(1, "driver init failed"); - } - - exit(0); + return interface; } -void -detect(int argc, char *argv[]) +int PX4IO::checkcrc(int argc, char *argv[]) { - if (g_dev != nullptr) { - errx(0, "already loaded"); + /* + check IO CRC against CRC of a file + */ + if (argc < 1) { + PX4_WARN("usage: px4io checkcrc filename"); + return 1; } - /* allocate the interface */ device::Device *interface = get_interface(); - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - errx(1, "driver allocation failed"); - } - - int ret = g_dev->detect(); - - delete g_dev; - g_dev = nullptr; - - if (ret) { - /* nonzero, error */ - exit(1); - - } else { - exit(0); + if (interface == nullptr) { + PX4_ERR("interface allocation failed"); + return 1; } -} - -void -checkcrc(int argc, char *argv[]) -{ - bool keep_running = false; - - if (g_dev == nullptr) { - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - PX4_ERR("driver allocation failed"); - exit(1); - } - } else { - /* its already running, don't kill the driver */ - keep_running = true; - } + PX4IO *dev = new PX4IO(interface); - /* - check IO CRC against CRC of a file - */ - if (argc < 2) { - PX4_WARN("usage: px4io checkcrc filename"); - exit(1); + if (dev == nullptr) { + delete interface; + PX4_ERR("driver allocation failed"); + return 1; } - int fd = open(argv[1], O_RDONLY); + int fd = ::open(argv[0], O_RDONLY); if (fd == -1) { - PX4_ERR("open of %s failed: %d", argv[1], errno); - exit(1); + delete dev; + PX4_ERR("open of %s failed: %d", argv[0], errno); + return 1; } const uint32_t app_size_max = 0xf000; @@ -3211,7 +1821,7 @@ checkcrc(int argc, char *argv[]) while (true) { uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); + int n = ::read(fd, buf, sizeof(buf)); if (n <= 0) { break; } @@ -3219,7 +1829,7 @@ checkcrc(int argc, char *argv[]) nbytes += n; } - close(fd); + ::close(fd); while (nbytes < app_size_max) { uint8_t b = 0xff; @@ -3227,238 +1837,153 @@ checkcrc(int argc, char *argv[]) nbytes++; } - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + int ret = dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (!keep_running) { - delete g_dev; - g_dev = nullptr; - } + delete dev; if (ret != OK) { - PX4_ERR("check CRC failed: %d, CRC: %u", ret, fw_crc); - exit(1); - - } else { - PX4_INFO("IO FW CRC match"); + PX4_WARN("check CRC failed: %d, CRC: %" PRIu32, ret, fw_crc); + return 1; } - exit(0); + PX4_INFO("IO FW CRC match"); + return 0; } -void -bind(int argc, char *argv[]) +int PX4IO::bind(int argc, char *argv[]) { int pulses; - if (g_dev == nullptr) { - errx(1, "px4io must be started first"); - } - - if (argc < 3) { - errx(0, "needs argument, use dsm2, dsmx or dsmx8"); + if (argc < 1) { + PX4_ERR("needs argument, use dsm2, dsmx or dsmx8"); + return 1; } - if (!strcmp(argv[2], "dsm2")) { + if (!strcmp(argv[0], "dsm2")) { pulses = DSM2_BIND_PULSES; - } else if (!strcmp(argv[2], "dsmx")) { + } else if (!strcmp(argv[0], "dsmx")) { pulses = DSMX_BIND_PULSES; - } else if (!strcmp(argv[2], "dsmx8")) { + } else if (!strcmp(argv[0], "dsmx8")) { pulses = DSMX8_BIND_PULSES; } else { - errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]); + PX4_ERR("unknown parameter %s, use dsm2, dsmx or dsmx8", argv[0]); + return 1; } // Test for custom pulse parameter - if (argc > 3) { - pulses = atoi(argv[3]); + if (argc > 1) { + pulses = atoi(argv[1]); } - g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - - exit(0); - + get_instance()->ioctl(nullptr, DSM_BIND_START, pulses); + return 0; } -void -monitor(void) +int PX4IO::task_spawn(int argc, char *argv[]) { - /* clear screen */ - printf("\033[2J"); - - unsigned cancels = 2; - - for (;;) { - pollfd fds[1]; - - fds[0].fd = 0; - fds[0].events = POLLIN; - - if (poll(fds, 1, 2000) < 0) { - errx(1, "poll fail"); - } - - if (fds[0].revents == POLLIN) { - /* control logic is to cancel with any key */ - char c; - (void)read(0, &c, 1); - - if (cancels-- == 0) { - printf("\033[2J\033[H"); /* move cursor home and clear screen */ - exit(0); - } - } - - if (g_dev != nullptr) { - - printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(false); - (void)g_dev->print_debug(); - printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + device::Device *interface = get_interface(); - } else { - errx(1, "driver not loaded, exiting"); - } + if (interface == nullptr) { + PX4_ERR("Failed to create interface"); + return -1; } -} - -void -lockdown(int argc, char *argv[]) -{ - if (g_dev != nullptr) { - - if (argc > 2 && !strcmp(argv[2], "disable")) { - - warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); - warnx("Press 'y' to enable, any other key to abort."); - - /* check if user wants to abort */ - char c; - - struct pollfd fds; - int ret; - hrt_abstime start = hrt_absolute_time(); - const unsigned long timeout = 5000000; - - while (hrt_elapsed_time(&start) < timeout) { - fds.fd = 0; /* stdin */ - fds.events = POLLIN; - ret = poll(&fds, 1, 0); - - if (ret > 0) { - - if (read(0, &c, 1) > 0) { - - if (c != 'y') { - exit(0); - - } else if (c == 'y') { - break; - } - } - } - - px4_usleep(10000); - } - - if (hrt_elapsed_time(&start) > timeout) { - errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); - } - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + PX4IO *instance = new PX4IO(interface); - warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - } else { - (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); - warnx("ACTUATORS ARE NOW SAFE IN HIL."); + if (instance->init() == PX4_OK) { + return PX4_OK; } } else { - errx(1, "driver not loaded, exiting"); + PX4_ERR("alloc failed"); } - exit(0); -} + delete instance; + _object.store(nullptr); + _task_id = -1; -} /* namespace */ + return PX4_ERROR; +} -int -px4io_main(int argc, char *argv[]) +int PX4IO::custom_command(int argc, char *argv[]) { - /* check for sufficient number of arguments */ - if (argc < 2) { - goto out; - } - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - errx(1, "PX4IO Not Supported"); - } + const char *verb = argv[0]; - if (!strcmp(argv[1], "start")) { - start(argc - 1, argv + 1); - } + if (!strcmp(verb, "checkcrc")) { + if (is_running()) { + PX4_ERR("io must be stopped"); + return 1; + } - if (!strcmp(argv[1], "detect")) { - detect(argc - 1, argv + 1); + return checkcrc(argc - 1, argv + 1); } - if (!strcmp(argv[1], "checkcrc")) { - checkcrc(argc - 1, argv + 1); - } + if (!strcmp(verb, "update")) { - if (!strcmp(argv[1], "update")) { + if (is_running()) { + PX4_ERR("io must be stopped"); + return 1; + } - constexpr unsigned MAX_RETRIES = 5; + constexpr unsigned MAX_RETRIES = 2; unsigned retries = 0; int ret = PX4_ERROR; while (ret != OK && retries < MAX_RETRIES) { - retries++; - // Sleep 200 ms before the next attempt - usleep(200 * 1000); + device::Device *interface = get_interface(); - if (g_dev == nullptr) { - /* allocate the interface */ - device::Device *interface = get_interface(); + if (interface == nullptr) { + PX4_ERR("interface allocation failed"); + return 1; + } - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); + PX4IO *dev = new PX4IO(interface); - if (g_dev == nullptr) { - delete interface; - errx(1, "driver allocation failed"); - } + if (dev == nullptr) { + delete interface; + PX4_ERR("driver allocation failed"); + return 1; } + retries++; + // Sleep 200 ms before the next attempt + usleep(200 * 1000); + // Try to reboot - ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, PX4IO_REBOOT_BL_MAGIC); - // tear down the px4io instance - delete g_dev; - g_dev = nullptr; + ret = dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, PX4IO_REBOOT_BL_MAGIC); + delete dev; if (ret != OK) { - PX4_ERR("reboot failed - %d, still attempting upgrade", ret); + PX4_WARN("reboot failed - %d, still attempting upgrade", ret); } - PX4IO_Uploader *up; - /* Assume we are using default paths */ const char *fn[4] = PX4IO_FW_SEARCH_PATHS; /* Override defaults if a path is passed on command line */ - if (argc > 2) { - fn[0] = argv[2]; + if (argc > 1) { + fn[0] = argv[1]; fn[1] = nullptr; } - up = new PX4IO_Uploader; - ret = up->upload(&fn[0]); - delete up; + PX4IO_Uploader *up = new PX4IO_Uploader(); + + if (!up) { + ret = -ENOMEM; + + } else { + ret = up->upload(&fn[0]); + delete up; + } } switch (ret) { @@ -3490,185 +2015,114 @@ px4io_main(int argc, char *argv[]) return ret; } - /* commands below here require a started driver */ - - if (g_dev == nullptr) { - errx(1, "not started"); - } - - if (!strcmp(argv[1], "safety_off")) { - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - - if (ret != OK) { - warnx("failed to disable safety"); - exit(1); - } - - exit(0); - } - - if (!strcmp(argv[1], "safety_on")) { - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0); - - if (ret != OK) { - warnx("failed to enable safety"); - exit(1); - } - - exit(0); - } - - if (!strcmp(argv[1], "recovery")) { - - /* - * Enable in-air restart support. - * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* stop the driver */ - delete g_dev; - g_dev = nullptr; - exit(0); - } - - - if (!strcmp(argv[1], "status")) { - warnx("loaded"); - g_dev->print_status(true); - - exit(0); + /* commands below here require a started driver */ + if (!is_running()) { + PX4_ERR("not running"); + return 1; } - if (!strcmp(argv[1], "debug")) { - if (argc <= 2) { - warnx("usage: px4io debug LEVEL"); - exit(1); + if (!strcmp(verb, "debug")) { + if (argc <= 1) { + PX4_ERR("usage: px4io debug LEVEL"); + return 1; } - if (g_dev == nullptr) { - warnx("not started"); - exit(1); - } - - uint8_t level = atoi(argv[2]); - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); + uint8_t level = atoi(argv[1]); + int ret = get_instance()->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { - warnx("SET_DEBUG failed: %d", ret); - exit(1); + PX4_ERR("SET_DEBUG failed: %d", ret); + return 1; } - warnx("SET_DEBUG %hhu OK", level); - exit(0); - } - - if (!strcmp(argv[1], "rx_dsm") || - !strcmp(argv[1], "rx_dsm_10bit") || - !strcmp(argv[1], "rx_dsm_11bit") || - !strcmp(argv[1], "rx_sbus") || - !strcmp(argv[1], "rx_ppm")) { - errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); - } - - if (!strcmp(argv[1], "monitor")) { - monitor(); - } - - if (!strcmp(argv[1], "bind")) { - bind(argc, argv); - } - - if (!strcmp(argv[1], "lockdown")) { - lockdown(argc, argv); + PX4_INFO("SET_DEBUG %" PRIu8 " OK", level); + return 0; } - if (!strcmp(argv[1], "sbus1_out")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); - - if (ret != 0) { - errx(ret, "S.BUS v1 failed"); + if (!strcmp(verb, "bind")) { + if (!is_running()) { + PX4_ERR("io must be running"); + return 1; } - exit(0); + return bind(argc - 1, argv + 1); } - if (!strcmp(argv[1], "sbus2_out")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + if (!strcmp(verb, "sbus1_out")) { + int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); if (ret != 0) { - errx(ret, "S.BUS v2 failed"); + PX4_ERR("S.BUS v1 failed (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "rssi_analog")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + if (!strcmp(verb, "sbus2_out")) { + int ret = get_instance()->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); if (ret != 0) { - errx(ret, "RSSI analog failed"); + PX4_ERR("S.BUS v2 failed (%i)", ret); + return 1; } - exit(0); + return 0; } - if (!strcmp(argv[1], "rssi_pwm")) { - /* we can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() - */ - int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); - - if (ret != 0) { - errx(ret, "RSSI PWM failed"); - } - - exit(0); + if (!strcmp(verb, "test_fmu_fail")) { + get_instance()->test_fmu_fail(true); + return 0; } - if (!strcmp(argv[1], "test_fmu_fail")) { - if (g_dev != nullptr) { - g_dev->test_fmu_fail(true); - exit(0); + if (!strcmp(verb, "test_fmu_ok")) { + get_instance()->test_fmu_fail(false); + return 0; + } - } else { + return print_usage("unknown command"); +} - errx(1, "px4io must be started first"); - } - } +int PX4IO::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Output driver communicating with the IO co-processor. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("px4io", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("checkcrc", "Check CRC for a firmware file against current code on IO"); + PRINT_MODULE_USAGE_ARG("", "Firmware file", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware"); + PRINT_MODULE_USAGE_ARG("", "Firmware file", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level"); + PRINT_MODULE_USAGE_ARG("", "0=disabled, 9=max verbosity", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind"); + PRINT_MODULE_USAGE_ARG("dsm2|dsmx|dsmx8", "protocol", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("sbus1_out", "enable sbus1 out"); + PRINT_MODULE_USAGE_COMMAND_DESCR("sbus2_out", "enable sbus2 out"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_fail", "test: turn off IO updates"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test_fmu_ok", "re-enable IO updates"); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - if (!strcmp(argv[1], "test_fmu_ok")) { - if (g_dev != nullptr) { - g_dev->test_fmu_fail(false); - exit(0); + return 0; +} - } else { - errx(1, "px4io must be started first"); - } +extern "C" __EXPORT int px4io_main(int argc, char *argv[]) +{ + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + PX4_ERR("PX4IO Not Supported"); + return -1; } - -out: - errx(1, "need a command, try 'start', 'stop', 'status', 'monitor', 'debug ',\n" - "'recovery', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" - "'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" - "'test_fmu_fail', 'test_fmu_ok'"); + return PX4IO::main(argc, argv); } diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index 542be13b88bf..84385e4216d8 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -45,11 +45,10 @@ /** * Set usage of IO board * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + * Can be used to use a configure the use of the IO board. * - * @boolean - * @min 0 - * @max 1 + * @value 0 IO PWM disabled (RC only) + * @value 1 IO enabled (RC & PWM) * @reboot_required true * @group System */ diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index a46478b929a0..efb7065c351f 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -51,24 +51,14 @@ device::Device PX4IO_serial::PX4IO_serial() : Device("PX4IO_serial"), - _pc_txns(perf_alloc(PC_ELAPSED, "io_txns")), -#if 0 - _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), - _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), - _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), - _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), - _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), - _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), - _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")), -#else - _pc_retries(nullptr), - _pc_timeouts(nullptr), - _pc_crcerrs(nullptr), - _pc_protoerrs(nullptr), - _pc_uerrs(nullptr), - _pc_idle(nullptr), - _pc_badidle(nullptr), -#endif + _pc_txns(perf_alloc(PC_ELAPSED, MODULE_NAME": txns")), + _pc_retries(perf_alloc(PC_COUNT, MODULE_NAME": retries")), + _pc_timeouts(perf_alloc(PC_COUNT, MODULE_NAME": timeouts")), + _pc_crcerrs(perf_alloc(PC_COUNT, MODULE_NAME": crcerrs")), + _pc_protoerrs(perf_alloc(PC_COUNT, MODULE_NAME": protoerrs")), + _pc_uerrs(perf_alloc(PC_COUNT, MODULE_NAME": uarterrs")), + _pc_idle(perf_alloc(PC_COUNT, MODULE_NAME": idle")), + _pc_badidle(perf_alloc(PC_COUNT, MODULE_NAME": badidle")), _bus_semaphore(SEM_INITIALIZER(0)) { g_interface = this; diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 217917477ba2..4010e0d5133e 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -355,9 +355,8 @@ int PX4IO_Uploader::get_sync(unsigned timeout) { uint8_t c[2]; - int ret; - ret = recv_byte_with_timeout(c, timeout); + int ret = recv_byte_with_timeout(c, timeout); if (ret != OK) { return ret; @@ -395,13 +394,11 @@ PX4IO_Uploader::sync() int PX4IO_Uploader::get_info(int param, uint32_t &val) { - int ret; - send(PROTO_GET_DEVICE); send(param); send(PROTO_EOC); - ret = recv_bytes((uint8_t *)&val, sizeof(val)); + int ret = recv_bytes((uint8_t *)&val, sizeof(val)); if (ret != OK) { return ret; @@ -419,7 +416,6 @@ PX4IO_Uploader::erase() return get_sync(10000); /* allow 10s timeout */ } - static int read_with_retry(int fd, void *buf, size_t n) { int ret; diff --git a/src/drivers/qshell/posix/CMakeLists.txt b/src/drivers/qshell/posix/CMakeLists.txt deleted file mode 100644 index d028be7983ed..000000000000 --- a/src/drivers/qshell/posix/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ -px4_add_module( - MODULE drivers__qshell__posix - MAIN qshell - SRCS - #qshell_main.cpp - qshell_start_posix.cpp - qshell.cpp - DEPENDS - ) - diff --git a/src/drivers/qshell/posix/Kconfig b/src/drivers/qshell/posix/Kconfig new file mode 100644 index 000000000000..91c2ab36dd2b --- /dev/null +++ b/src/drivers/qshell/posix/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_QSHELL_POSIX + bool "posix" + default n + ---help--- + Enable support for posix \ No newline at end of file diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp deleted file mode 100644 index 6ac9b0426686..000000000000 --- a/src/drivers/qshell/posix/qshell.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * - * 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 qshell.cpp - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "qshell.h" - -px4::AppState QShell::appState; - -int QShell::main(std::vector argList) -{ - int ret = _send_cmd(argList); - - if (ret != 0) { - PX4_ERR("Could not send command"); - return -1; - } - - ret = _wait_for_retval(); - - if (ret != 0) { - PX4_ERR("Could not get return value"); - return -1; - } - - return 0; -} - -int QShell::_send_cmd(std::vector &argList) -{ - // Let's use a sequence number to check if a return value belongs to the - // command that we just sent and not a previous one that we assumed that - // it had timed out. - ++_current_sequence; - - qshell_req_s qshell_req{}; - std::string cmd; - - for (size_t i = 0; i < argList.size(); i++) { - cmd += argList[i]; - - if (i < argList.size() - 1) { - cmd += " "; - } - } - - if (cmd.size() >= qshell_req.MAX_STRLEN) { - PX4_ERR("Command too long: %d >= %d", (int) cmd.size(), (int) qshell_req.MAX_STRLEN); - return -1; - } - - PX4_INFO("Send cmd: '%s'", cmd.c_str()); - - qshell_req.strlen = cmd.size(); - strcpy((char *)qshell_req.cmd, cmd.c_str()); - qshell_req.request_sequence = _current_sequence; - - qshell_req.timestamp = hrt_absolute_time(); - _qshell_req_pub.publish(qshell_req); - - return 0; -} - -int QShell::_wait_for_retval() -{ - const hrt_abstime time_started_us = hrt_absolute_time(); - - while (hrt_elapsed_time(&time_started_us) < 3000000) { - qshell_retval_s retval; - - if (_qshell_retval_sub.update(&retval)) { - if (retval.return_sequence != _current_sequence) { - PX4_WARN("Ignoring return value with wrong sequence"); - - } else { - if (retval.return_value) { - PX4_WARN("cmd returned with: %d", retval.return_value); - } - - return 0; - } - } - - px4_usleep(1000); - } - - PX4_ERR("command timed out"); - return -1; -} diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h deleted file mode 100644 index 8aa8cd9e54b0..000000000000 --- a/src/drivers/qshell/posix/qshell.h +++ /dev/null @@ -1,71 +0,0 @@ -/**************************************************************************** - * - * 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 qshell.h - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#pragma once - -#include -#include -#include -#include -#include - -#include -#include - -class QShell -{ -public: - QShell() = default; - ~QShell() = default; - - int main(std::vector argList); - - static px4::AppState appState; /* track requests to terminate app */ - -private: - int _send_cmd(std::vector &argList); - int _wait_for_retval(); - - uORB::Publication _qshell_req_pub{ORB_ID(qshell_req)}; - - uORB::Subscription _qshell_retval_sub{ORB_ID(qshell_retval)}; - - uint32_t _current_sequence{0}; -}; diff --git a/src/drivers/qshell/posix/qshell_start_posix.cpp b/src/drivers/qshell/posix/qshell_start_posix.cpp deleted file mode 100644 index e8b681c0b621..000000000000 --- a/src/drivers/qshell/posix/qshell_start_posix.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * 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 qshell_start_posix.cpp - * Send shell commands to qurt - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//using namespace px4; - -extern "C" __EXPORT int qshell_main(int argc, char *argv[]); - -static void usage() -{ - PX4_DEBUG("usage: qshell cmd [args]"); -} - -int qshell_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - std::vector argList; - - for (int i = 1; i < argc; i++) { - argList.push_back(argv[i]); - } - - - QShell qshell; - return qshell.main(argList); -} diff --git a/src/drivers/qshell/qurt/CMakeLists.txt b/src/drivers/qshell/qurt/CMakeLists.txt deleted file mode 100644 index 1133de136189..000000000000 --- a/src/drivers/qshell/qurt/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ -px4_add_module( - MODULE drivers__qshell__qurt - MAIN qshell - SRCS - qshell_main.cpp - qshell_start_qurt.cpp - qshell.cpp - DEPENDS - ) diff --git a/src/drivers/qshell/qurt/Kconfig b/src/drivers/qshell/qurt/Kconfig new file mode 100644 index 000000000000..73b3b81ca9ae --- /dev/null +++ b/src/drivers/qshell/qurt/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_QSHELL_QURT + bool "qurt" + default n + ---help--- + Enable support for qurt \ No newline at end of file diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp deleted file mode 100644 index 9cd28c64fc6a..000000000000 --- a/src/drivers/qshell/qurt/qshell.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/**************************************************************************** - * - * 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 qshell.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#define MAX_ARGS 8 // max number of whitespace separated args after app name - -px4::AppState QShell::appState; - -QShell::QShell() -{ - init_app_map(m_apps); -} - -int QShell::main() -{ - appState.setRunning(true); - int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req)); - - if (sub_qshell_req == PX4_ERROR) { - PX4_ERR("Error subscribing to qshell_req topic"); - return -1; - } - - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = sub_qshell_req; - fds[0].events = POLLIN; - - while (!appState.exitRequested()) { - - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - - if (pret > 0 && fds[0].revents & POLLIN) { - - orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req); - - PX4_INFO("qshell gotten: %s", m_qshell_req.cmd); - char current_char; - std::string arg; - std::vector appargs; - - for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) { - current_char = m_qshell_req.cmd[str_idx]; - - if (isspace(current_char)) { // split at spaces - if (arg.length()) { - appargs.push_back(arg); - arg = ""; - } - - } else { - arg += current_char; - } - } - - appargs.push_back(arg); // push last argument - - qshell_retval_s retval{}; - retval.return_value = run_cmd(appargs); - retval.return_sequence = m_qshell_req.request_sequence; - - if (retval.return_value) { - PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd); - - } else { - PX4_INFO("Ok executing command: %s", m_qshell_req.cmd); - } - - retval.timestamp = hrt_absolute_time(); - _qshell_retval_pub.publish(retval); - - } else if (pret == 0) { - // Timing out is fine. - } else { - // Something is wrong. - usleep(10000); - } - } - - appState.setRunning(false); - return 0; -} - -int QShell::run_cmd(const std::vector &appargs) -{ - // command is appargs[0] - std::string command = appargs[0]; - - if (command.compare("help") == 0) { - list_builtins(m_apps); - return 0; - } - - //replaces app.find with iterator code to avoid null pointer exception - for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) { - if (it->first == command) { - // one for command name, one for null terminator - const char *arg[MAX_ARGS + 2]; - - unsigned int i = 0; - - if (appargs.size() > MAX_ARGS + 1) { - PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1)); - return 1; - } - - while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { - arg[i] = (char *)appargs[i].c_str(); - PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]); - ++i; - } - - arg[i] = (char *)0; - - //PX4_DEBUG_PRINTF(i); - if (m_apps[command] == NULL) { - PX4_ERR("Null function !!\n"); - - } else { - return m_apps[command](i, (char **)arg); - } - - } - } - - PX4_ERR("Command %s not found", command.c_str()); - return 1; -} diff --git a/src/drivers/qshell/qurt/qshell.h b/src/drivers/qshell/qurt/qshell.h deleted file mode 100644 index aa5ee41d6fca..000000000000 --- a/src/drivers/qshell/qurt/qshell.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * 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 qshell.h - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "apps.h" - -class QShell -{ -public: - QShell(); - ~QShell() {} - - int main(); - int run_cmd(const std::vector &appargs); - - static px4::AppState appState; /* track requests to terminate app */ - -private: - - uORB::Publication _qshell_retval_pub{ORB_ID(qshell_retval)}; - - qshell_req_s m_qshell_req{}; - - apps_map_type m_apps; -}; diff --git a/src/drivers/qshell/qurt/qshell_main.cpp b/src/drivers/qshell/qurt/qshell_main.cpp deleted file mode 100644 index 56010acf8888..000000000000 --- a/src/drivers/qshell/qurt/qshell_main.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/**************************************************************************** - * - * 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 qshell_main.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include -#include -#include -#include "qshell.h" - -extern "C" __EXPORT int qshell_entry(int argc, char **argv); - -int qshell_entry(int argc, char **argv) -{ - px4::init(argc, argv, "qshell"); - - PX4_DEBUG("qshell"); - QShell qshell; - qshell.main(); - - PX4_DEBUG("goodbye"); - return 0; -} diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp deleted file mode 100644 index 18abd360c615..000000000000 --- a/src/drivers/qshell/qurt/qshell_start_qurt.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * 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 qshell_start_qurt.cpp - * Listener for shell commands from posix - * - * @author Nicolas de Palezieux - */ - -#include "qshell.h" -#include -#include -#include -#include -#include -#include - -static int daemon_task; /* Handle of deamon task / thread */ - -//using namespace px4; - -extern "C" __EXPORT int qshell_main(int argc, char *argv[]); - -int qshell_entry(int argc, char **argv) -{ - //px4::init(argc, argv, "qshell"); - - PX4_DEBUG("qshell entry....."); - QShell qshell; - qshell.main(); - - PX4_DEBUG("goodbye"); - return 0; -} - -static void usage() -{ - PX4_INFO("usage: qshell {start|stop|status}"); -} - -int qshell_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (QShell::appState.isRunning()) { - PX4_INFO("already running"); - /* this is not an error */ - return 0; - } - - PX4_DEBUG("before starting the qshell_entry task"); - - daemon_task = px4_task_spawn_cmd("qshell", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - qshell_entry, - (char *const *)argv); - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - QShell::appState.requestExit(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (QShell::appState.isRunning()) { - PX4_INFO("is running"); - - } else { - PX4_INFO("not started"); - } - - return 0; - } - - usage(); - return 1; -} diff --git a/src/drivers/rc_input/Kconfig b/src/drivers/rc_input/Kconfig new file mode 100644 index 000000000000..1ada2e84f4d8 --- /dev/null +++ b/src/drivers/rc_input/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_INPUT + bool "rc_input" + default n + ---help--- + Enable support for rc_input \ No newline at end of file diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 3c9fead25ebd..c21ace77d5bf 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. + * 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 @@ -36,6 +36,8 @@ #include "crsf_telemetry.h" #include +#include + using namespace time_literals; constexpr char const *RCInput::RC_SCAN_STRING[]; @@ -46,12 +48,6 @@ RCInput::RCInput(const char *device) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) { - // rc input, published to ORB - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - - // initialize it as RC lost - _rc_in.rc_lost = true; - // initialize raw_rc values and count for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) { _raw_rc_values[i] = UINT16_MAX; @@ -61,6 +57,10 @@ RCInput::RCInput(const char *device) : strncpy(_device, device, sizeof(_device) - 1); _device[sizeof(_device) - 1] = '\0'; } + + if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) { + _rc_scan_state = static_cast(_param_rc_input_proto.get()); + } } RCInput::~RCInput() @@ -110,6 +110,8 @@ RCInput::init() px4_arch_unconfiggpio(GPIO_PPM_IN); #endif // GPIO_PPM_IN + rc_io_invert(false); + return 0; } @@ -121,15 +123,15 @@ RCInput::task_spawn(int argc, char *argv[]) int myoptind = 1; int ch; const char *myoptarg = nullptr; - const char *device = nullptr; + const char *device_name = nullptr; #if defined(RC_SERIAL_PORT) - device = RC_SERIAL_PORT; + device_name = RC_SERIAL_PORT; #endif // RC_SERIAL_PORT while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'd': - device = myoptarg; + device_name = myoptarg; break; case '?': @@ -147,24 +149,31 @@ RCInput::task_spawn(int argc, char *argv[]) return -1; } - if (device == nullptr) { - PX4_ERR("valid device required"); - return PX4_ERROR; - } + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + RCInput *instance = new RCInput(device_name); - RCInput *instance = new RCInput(device); + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return PX4_ERROR; - } + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleOnInterval(_current_update_interval); - _object.store(instance); - _task_id = task_id_is_work_queue; + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); - instance->ScheduleOnInterval(_current_update_interval); + } else { + PX4_INFO("valid device required"); + } + } - return PX4_OK; + return PX4_ERROR; } void @@ -242,9 +251,21 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, void RCInput::set_rc_scan_state(RC_SCAN newState) { - PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]); + if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE) + && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) { + + _rc_scan_state = static_cast(_param_rc_input_proto.get()); + + } else if (_param_rc_input_proto.get() < 0) { + // only auto change if RC_INPUT_PROTO set to auto (-1) + PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]); + _rc_scan_state = newState; + + } else { + _rc_scan_state = RC_SCAN::RC_SCAN_NONE; + } + _rc_scan_begin = 0; - _rc_scan_state = newState; _rc_scan_locked = false; } @@ -254,7 +275,14 @@ void RCInput::rc_io_invert(bool invert) // and if not use an IOCTL if (!board_rc_invert_input(_device, invert)) { #if defined(TIOCSINVERT) - ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0); + + if (invert) { + ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX); + + } else { + ioctl(_rcs_fd, TIOCSINVERT, 0); + } + #endif // TIOCSINVERT } } @@ -383,31 +411,30 @@ void RCInput::Run() bool rc_updated = false; // This block scans for a supported serial RC input and locks onto the first one found - // Scan for 300 msec, then switch protocol - constexpr hrt_abstime rc_scan_max = 300_ms; + // Scan for 500 msec, then switch protocol + constexpr hrt_abstime rc_scan_max = 500_ms; unsigned frame_drops = 0; - if (_report_lock && _rc_scan_locked) { - _report_lock = false; - PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); - } - - int newBytes = 0; - // TODO: needs work (poll _rcs_fd) // int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 100); // then update priority to SCHED_PRIORITY_FAST_DRIVER // read all available data from the serial RC input UART // read all available data from the serial RC input UART - newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_BUFFER_SIZE); + int newBytes = ::read(_rcs_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE); if (newBytes > 0) { _bytes_rx += newBytes; } + const bool rc_scan_locked = _rc_scan_locked; + switch (_rc_scan_state) { + case RC_SCAN_NONE: + // do nothing + break; + case RC_SCAN_SBUS: if (_rc_scan_begin == 0) { _rc_scan_begin = cycle_timestamp; @@ -415,6 +442,10 @@ void RCInput::Run() sbus_config(_rcs_fd, board_rc_singlewire(_device)); rc_io_invert(true); + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); + } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -437,6 +468,7 @@ void RCInput::Run() } else { // Scan the next protocol + rc_io_invert(false); set_rc_scan_state(RC_SCAN_DSM); } @@ -445,9 +477,12 @@ void RCInput::Run() case RC_SCAN_DSM: if (_rc_scan_begin == 0) { _rc_scan_begin = cycle_timestamp; - // // Configure serial port for DSM + // Configure serial port for DSM dsm_config(_rcs_fd); - rc_io_invert(false); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -481,7 +516,10 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for DSM dsm_config(_rcs_fd); - rc_io_invert(false); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -494,7 +532,7 @@ void RCInput::Run() for (unsigned i = 0; i < (unsigned)newBytes; i++) { /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; + st24_rssi = input_rc_s::RSSI_MAX; rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &lost_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); } @@ -529,7 +567,10 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for DSM dsm_config(_rcs_fd); - rc_io_invert(false); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -543,7 +584,7 @@ void RCInput::Run() for (unsigned i = 0; i < (unsigned)newBytes; i++) { /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; + sumd_rssi = input_rc_s::RSSI_MAX; rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count, &_raw_rc_count, _raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS, &sumd_failsafe)); } @@ -571,8 +612,6 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure timer input pin for CPPM px4_arch_configgpio(GPIO_PPM_IN); - rc_io_invert(false); - ioctl(_rcs_fd, TIOCSINVERT, 0); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -606,7 +645,10 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for CRSF crsf_config(_rcs_fd); - rc_io_invert(false); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -621,16 +663,15 @@ void RCInput::Run() _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0); - // Enable CRSF Telemetry only on the Omnibus, because on Pixhawk (-related) boards - // we cannot write to the RC UART - // It might work on FMU-v5. Or another option is to use a different UART port -#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD + // on Pixhawk (-related) boards we cannot write to the RC UART + // another option is to use a different UART port +#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT if (!_rc_scan_locked && !_crsf_telemetry) { _crsf_telemetry = new CRSFTelemetry(_rcs_fd); } -#endif /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */ +#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ _rc_scan_locked = true; @@ -652,7 +693,10 @@ void RCInput::Run() _rc_scan_begin = cycle_timestamp; // Configure serial port for GHST ghst_config(_rcs_fd); - rc_io_invert(false); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + memset(_rcs_buf, 0, sizeof(_rcs_buf)); } else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) { @@ -669,13 +713,15 @@ void RCInput::Run() fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); // ghst telemetry works on fmu-v5 - // on other Pixhawk (-related) boards it does not work because - // we cannot write to the RC UART + // on other Pixhawk (-related) boards we cannot write to the RC UART + // another option is to use a different UART port +#ifdef BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT if (!_rc_scan_locked && !_ghst_telemetry) { _ghst_telemetry = new GHSTTelemetry(_rcs_fd); } +#endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ _rc_scan_locked = true; @@ -700,9 +746,23 @@ void RCInput::Run() _to_input_rc.publish(_rc_in); - } else if (!rc_updated && ((hrt_absolute_time() - _rc_in.timestamp_last_signal) > 1_s)) { + } else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) { _rc_scan_locked = false; } + + if (!rc_scan_locked && _rc_scan_locked) { + PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); + } + + // set RC_INPUT_PROTO if RC successfully locked for > 3 seconds + if (!_armed && rc_updated && _rc_scan_locked + && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s) + && (_param_rc_input_proto.get() < 0) + ) { + // RC_INPUT_PROTO auto => locked selection + _param_rc_input_proto.set(_rc_scan_state); + _param_rc_input_proto.commit(); + } } } @@ -777,17 +837,20 @@ int RCInput::custom_command(int argc, char *argv[]) int RCInput::print_status() { - PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval); + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); if (_device[0] != '\0') { PX4_INFO("UART device: %s", _device); - PX4_INFO("UART RX bytes: %u", _bytes_rx); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); } PX4_INFO("RC state: %s: %s", _rc_scan_locked ? "found" : "searching for signal", RC_SCAN_STRING[_rc_scan_state]); if (_rc_scan_locked) { switch (_rc_scan_state) { + case RC_SCAN_NONE: + break; + case RC_SCAN_CRSF: PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); break; @@ -822,14 +885,18 @@ int RCInput::print_status() } #if ADC_RC_RSSI_CHANNEL - PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f)); + + if (_analog_rc_rssi_stable) { + PX4_INFO("vrssi: %dmV", (int)(_analog_rc_rssi_volt * 1000.0f)); + } + #endif perf_print_counter(_cycle_perf); perf_print_counter(_publish_interval_perf); if (hrt_elapsed_time(&_rc_in.timestamp) < 1_s) { - print_message(_rc_in); + print_message(ORB_ID(input_rc), _rc_in); } return 0; diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index e4aaac410826..efea2ba23185 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * 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 @@ -38,7 +38,6 @@ #include #include #include -#include #include #include #include @@ -92,21 +91,23 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch private: enum RC_SCAN { - RC_SCAN_PPM = 0, - RC_SCAN_SBUS, - RC_SCAN_DSM, - RC_SCAN_SUMD, - RC_SCAN_ST24, - RC_SCAN_CRSF, - RC_SCAN_GHST + RC_SCAN_NONE = 0, + RC_SCAN_PPM = 1, + RC_SCAN_SBUS = 2, + RC_SCAN_DSM = 3, + RC_SCAN_ST24 = 5, + RC_SCAN_SUMD = 4, + RC_SCAN_CRSF = 6, + RC_SCAN_GHST = 7, } _rc_scan_state{RC_SCAN_SBUS}; - static constexpr char const *RC_SCAN_STRING[7] { + static constexpr char const *RC_SCAN_STRING[] { + "None", "PPM", "SBUS", "DSM", - "SUMD", "ST24", + "SUMD", "CRSF", "GHST" }; @@ -130,7 +131,6 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch bool _initialized{false}; bool _rc_scan_locked{false}; - bool _report_lock{true}; static constexpr unsigned _current_update_interval{4000}; // 250 Hz @@ -153,7 +153,8 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch int _rcs_fd{-1}; char _device[20] {}; ///< device / serial port path - uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {}; + static constexpr size_t RC_MAX_BUFFER_SIZE{SBUS_BUFFER_SIZE}; + uint8_t _rcs_buf[RC_MAX_BUFFER_SIZE] {}; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; uint16_t _raw_rc_count{}; @@ -168,6 +169,7 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch DEFINE_PARAMETERS( (ParamInt) _param_rc_rssi_pwm_chan, (ParamInt) _param_rc_rssi_pwm_min, - (ParamInt) _param_rc_rssi_pwm_max + (ParamInt) _param_rc_rssi_pwm_max, + (ParamInt) _param_rc_input_proto ) }; diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 37a5092cd39b..4cec3f82947c 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -162,11 +162,6 @@ bool CRSFTelemetry::send_flight_mode() flight_mode = "Auto"; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - flight_mode = "Failure"; - break; - case vehicle_status_s::NAVIGATION_STATE_ACRO: flight_mode = "Acro"; break; diff --git a/src/drivers/rc_input/module.yaml b/src/drivers/rc_input/module.yaml index ae3c04ad4256..d0f9040a6ff8 100644 --- a/src/drivers/rc_input/module.yaml +++ b/src/drivers/rc_input/module.yaml @@ -1,11 +1,34 @@ module_name: RC Input Driver +parameters: + - group: RC Input + definitions: + RC_INPUT_PROTO: + description: + short: RC input protocol + long: | + Select your RC input protocol or auto to scan. + category: System + type: enum + values: + -1: Auto + 0: None + 1: PPM + 2: SBUS + 3: DSM + 4: ST24 + 5: SUMD + 6: CRSF + 7: GHST + min: -1 + max: 7 + default: -1 + serial_config: - command: set RC_INPUT_ARGS "-d ${SERIAL_DEV}" port_config_param: name: RC_PORT_CONFIG group: Serial default: RC - depends_on_port: RC description_extended: | Setting this to 'Disabled' will use a board-specific default port for RC input. diff --git a/src/drivers/roboclaw/Kconfig b/src/drivers/roboclaw/Kconfig new file mode 100644 index 000000000000..9315e757bcaf --- /dev/null +++ b/src/drivers/roboclaw/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ROBOCLAW + bool "roboclaw" + default n + ---help--- + Enable support for roboclaw \ No newline at end of file diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index c59a91e73919..89c2ee783e7c 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -491,7 +491,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, int count = write(_uart, buf, wbytes); if (count < (int) wbytes) { // Did not successfully send all bytes. - PX4_ERR("Only wrote %d out of %d bytes", count, (int) wbytes); + PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); return -1; } @@ -568,7 +568,7 @@ void RoboClaw::_parameters_update() orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); } - int baudRate; + int32_t baudRate; param_get(_param_handles.serial_baud_rate, &baudRate); switch (baudRate) { diff --git a/src/drivers/rpi_rc_in/Kconfig b/src/drivers/rpi_rc_in/Kconfig new file mode 100644 index 000000000000..c496b033c9e9 --- /dev/null +++ b/src/drivers/rpi_rc_in/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RPI_RC_IN + bool "rpi_rc_in" + default n + ---help--- + Enable support for rpi_rc_in \ No newline at end of file diff --git a/src/drivers/rpm/Kconfig b/src/drivers/rpm/Kconfig new file mode 100644 index 000000000000..c4add0d44c5f --- /dev/null +++ b/src/drivers/rpm/Kconfig @@ -0,0 +1,7 @@ +menuconfig DRIVERS_RPM + bool "rpm" + default n + ---help--- + Enable support for rpm + +rsource "*/Kconfig" diff --git a/src/drivers/rpm/pcf8583/PCF8583.cpp b/src/drivers/rpm/pcf8583/PCF8583.cpp index 118d09e1319c..8ab28995b097 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.cpp +++ b/src/drivers/rpm/pcf8583/PCF8583.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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,58 +33,101 @@ #include "PCF8583.hpp" -PCF8583::PCF8583(I2CSPIBusOption bus_option, const int bus, int bus_frequency) : - I2C(DRV_SENS_DEVTYPE_PCF8583, MODULE_NAME, bus, PCF8583_BASEADDR_DEFAULT, bus_frequency), +PCF8583::PCF8583(const I2CSPIDriverConfig &config) : + I2C(config), ModuleParams(nullptr), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus) + I2CSPIDriver(config) { } int PCF8583::init() { - set_device_address(_param_pcf8583_addr.get()); - if (I2C::init() != PX4_OK) { return PX4_ERROR; } - PX4_DEBUG("addr: %d, pool: %d, reset: %d, magenet: %d", get_device_address(), _param_pcf8583_pool.get(), + PX4_DEBUG("addr: %" PRId8 ", pool: %" PRId32 ", reset: %" PRId32 ", magenet: %" PRId32, get_device_address(), + _param_pcf8583_pool.get(), _param_pcf8583_reset.get(), _param_pcf8583_magnet.get()); - // set counter mode - setRegister(0x00, 0b00100000); - - // start measurement - resetCounter(); - + initCounter(); ScheduleOnInterval(_param_pcf8583_pool.get()); + _rpm_pub.advertise(); return PX4_OK; } int PCF8583::probe() { + setRegister(0x00, 0b00100000); + + uint8_t s = readRegister(0x00); + PX4_DEBUG("status register: %" PRId8 " fail_count: %" PRId8, s, _tranfer_fail_count); + + // PCF8583 contains free RAM registers + // This checks if I2C devices contains this RAM memory registers + // Some values are stored into this registers + // then it is vertified that the entered values fit. + setRegister(0x04, 10); + setRegister(0x05, 10); + setRegister(0x06, 10); + setRegister(0x0c, 5); + setRegister(0x0d, 5); + setRegister(0x0e, 5); + uint32_t tmp{0}; + + // check values stored in free RAM parts + tmp += readRegister(0x04); + tmp += readRegister(0x05); + tmp += readRegister(0x06); + tmp += readRegister(0x0c); + tmp += readRegister(0x0d); + tmp += readRegister(0x0e); + + if (tmp != 45) { + return PX4_ERROR; + } + return PX4_OK; } -int PCF8583::getCounter() +void PCF8583::initCounter() { + // set counter mode + _tranfer_fail_count = 0; + setRegister(0x00, 0b00100000); + resetCounter(); + +} + +uint32_t PCF8583::getCounter() +{ + // Counter value is stored in 9 words + // in 3 register as BCD value uint8_t a = readRegister(0x01); uint8_t b = readRegister(0x02); uint8_t c = readRegister(0x03); - return int((a & 0x0f) * 1 + ((a & 0xf0) >> 4) * 10 + (b & 0x0f) * 100 + ((b & 0xf0) >> 4) * 1000 + - (c & 0x0f) * 10000 + ((c & 0xf0) >> 4) * 1000000); + return uint32_t( + hiWord(a) * 1u + loWord(a) * 10u + + hiWord(b) * 100u + loWord(b) * 1000u + + hiWord(c) * 10000u + loWord(c) * 1000000u); } void PCF8583::resetCounter() { + _last_measurement_time = hrt_absolute_time(); setRegister(0x01, 0x00); setRegister(0x02, 0x00); setRegister(0x03, 0x00); + _count = 0; + _last_reset_time = _last_measurement_time; + _reset_count ++; } + +// Configure PCF8583 driver into counting mode void PCF8583::setRegister(uint8_t reg, uint8_t value) { uint8_t buff[2]; @@ -92,8 +135,13 @@ void PCF8583::setRegister(uint8_t reg, uint8_t value) buff[1] = value; int ret = transfer(buff, 2, nullptr, 0); + if (reg == 0x00) { + _last_config_register_content = value; + } + if (PX4_OK != ret) { PX4_DEBUG("setRegister : i2c::transfer returned %d", ret); + _tranfer_fail_count++; } } @@ -104,6 +152,7 @@ uint8_t PCF8583::readRegister(uint8_t reg) if (PX4_OK != ret) { PX4_DEBUG("readRegister : i2c::transfer returned %d", ret); + _tranfer_fail_count++; } return rcv; @@ -112,34 +161,61 @@ uint8_t PCF8583::readRegister(uint8_t reg) void PCF8583::RunImpl() { // read sensor and compute frequency - int oldcount = _count; - uint64_t oldtime = _last_measurement_time; + int32_t oldcount = _count; + + int32_t diffTime = hrt_elapsed_time(&_last_measurement_time); + + // check if delay is enought + if (diffTime < _param_pcf8583_pool.get() / 2) { + PX4_ERR("pcf8583 loop called too early"); + return; + } _count = getCounter(); _last_measurement_time = hrt_absolute_time(); - int diffCount = _count - oldcount; - uint64_t diffTime = _last_measurement_time - oldtime; + int32_t diffCount = _count - oldcount; - if (_param_pcf8583_reset.get() < _count + diffCount) { + // check if there is enought space in counter + // Otherwise, reset counter + if (diffCount > (999999 - oldcount)) { + PX4_ERR("pcf8583 RPM register overflow"); resetCounter(); - _last_measurement_time = hrt_absolute_time(); - _count = 0; + return; + } + + //check if device failed or reset + uint8_t s = readRegister(0x00); + + if (_tranfer_fail_count > 0 || s != 0b00100000 || diffCount < 0) { + PX4_ERR("pcf8583 RPM sensor restart: fail count %d, status: %d, diffCount: %ld", + _tranfer_fail_count, s, diffCount); + initCounter(); + return; } - float indicated_rpm = (float)diffCount / _param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f; + // 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; - // publish + // publish data to uorb rpm_s msg{}; msg.indicated_frequency_rpm = indicated_rpm; msg.estimated_accurancy_rpm = estimated_accurancy; msg.timestamp = hrt_absolute_time(); _rpm_pub.publish(msg); + + //check counter range + if (_param_pcf8583_reset.get() < diffCount + (int)_count) { + resetCounter(); + } } void PCF8583::print_status() { I2CSPIDriverBase::print_status(); - PX4_INFO("poll interval: %d us", _param_pcf8583_pool.get()); + PX4_INFO("poll interval: %" PRId32 " us", _param_pcf8583_pool.get()); + PX4_INFO("Last reset %.3fs ago, Count of resets: %d", (double)(hrt_absolute_time() - _last_reset_time) / 1000000.0, + _reset_count); + PX4_INFO("Last count %ld", _count); } diff --git a/src/drivers/rpm/pcf8583/PCF8583.hpp b/src/drivers/rpm/pcf8583/PCF8583.hpp index 91f5f47fc4bd..453befe407e4 100644 --- a/src/drivers/rpm/pcf8583/PCF8583.hpp +++ b/src/drivers/rpm/pcf8583/PCF8583.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020, 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 @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -57,11 +58,9 @@ class PCF8583 : public device::I2C, public ModuleParams, public I2CSPIDriver { public: - PCF8583(I2CSPIBusOption bus_option, const int bus, int bus_frequency); + PCF8583(const I2CSPIDriverConfig &config); ~PCF8583() override = default; - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); static void print_usage(); void RunImpl(); @@ -73,19 +72,26 @@ class PCF8583 : public device::I2C, public ModuleParams, public I2CSPIDriver> 4); } + + uint32_t _count{0}; + uint16_t _reset_count{0}; hrt_abstime _last_measurement_time{0}; + hrt_abstime _last_reset_time{0}; + int _tranfer_fail_count{0}; + uint8_t _last_config_register_content{0x00}; - uORB::Publication _rpm_pub{ORB_ID(rpm)}; + uORB::PublicationMulti _rpm_pub{ORB_ID(rpm)}; DEFINE_PARAMETERS( - (ParamInt) _param_pcf8583_addr, (ParamInt) _param_pcf8583_pool, (ParamInt) _param_pcf8583_reset, (ParamInt) _param_pcf8583_magnet diff --git a/src/drivers/rpm/pcf8583/parameters.c b/src/drivers/rpm/pcf8583/parameters.c index 1ed06b50fb77..bf2d41b6fdde 100644 --- a/src/drivers/rpm/pcf8583/parameters.c +++ b/src/drivers/rpm/pcf8583/parameters.c @@ -31,6 +31,21 @@ * ****************************************************************************/ +/** +* PCF8583 eneable driver +* +* Run PCF8583 driver automatically +* +* @reboot_required true +* @min 0 +* @max 1 +* @group Sensors +* @value 0 Disabled +* @value 1 Eneabled +*/ +PARAM_DEFINE_INT32(SENS_EN_PCF8583, 0); + + /** * PCF8583 rotorfreq (i2c) pool interval * @@ -42,16 +57,6 @@ */ PARAM_DEFINE_INT32(PCF8583_POOL, 1000000); -/** - * PCF8583 rotorfreq (i2c) i2c address - * - * @reboot_required true - * @group Sensors - * @value 80 Address 0x50 (80) - * @value 81 Address 0x51 (81) - */ -PARAM_DEFINE_INT32(PCF8583_ADDR, 80); - /** * PCF8583 rotorfreq (i2c) pulse reset value * diff --git a/src/drivers/rpm/pcf8583/pcf8583_main.cpp b/src/drivers/rpm/pcf8583/pcf8583_main.cpp index a958e0430587..be584d79696c 100644 --- a/src/drivers/rpm/pcf8583/pcf8583_main.cpp +++ b/src/drivers/rpm/pcf8583/pcf8583_main.cpp @@ -40,33 +40,23 @@ void PCF8583::print_usage() { PRINT_MODULE_USAGE_NAME("pcf8583", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("rpm_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x50); + PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *PCF8583::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - PCF8583 *instance = new PCF8583(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (instance->init() != PX4_OK) { - delete instance; - return nullptr; - } - - return instance; -} extern "C" __EXPORT int pcf8583_main(int argc, char *argv[]) { using ThisDriver = PCF8583; BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 400000; + cli.default_i2c_frequency = 100000; + + //int32_t addr{0x50}; + cli.i2c_address = 0x50; + cli.support_keep_running = true; const char *verb = cli.parseDefaultArguments(argc, argv); diff --git a/src/drivers/rpm/rpm_simulator/Kconfig b/src/drivers/rpm/rpm_simulator/Kconfig new file mode 100644 index 000000000000..8b96677d0392 --- /dev/null +++ b/src/drivers/rpm/rpm_simulator/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RPM_RPM_SIMULATOR + bool "rpm_simulator" + default n + ---help--- + Enable support for rpm_simulator \ No newline at end of file diff --git a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp index 6253f6e4d354..e48754b20b63 100644 --- a/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp +++ b/src/drivers/rpm/rpm_simulator/rpm_simulator.cpp @@ -72,7 +72,7 @@ int rpm_simulator_main(int argc, char *argv[]) // Publish data and let the user know what was published rpm_pub.publish(rpm); - print_message(rpm); + print_message(ORB_ID(rpm), rpm); return 0; } diff --git a/src/drivers/safety_button/CMakeLists.txt b/src/drivers/safety_button/CMakeLists.txt index f1fb9c3e6b0a..06083e96a00c 100644 --- a/src/drivers/safety_button/CMakeLists.txt +++ b/src/drivers/safety_button/CMakeLists.txt @@ -37,6 +37,6 @@ px4_add_module( SRCS SafetyButton.cpp DEPENDS - circuit_breaker + button_publisher px4_work_queue ) diff --git a/src/drivers/safety_button/Kconfig b/src/drivers/safety_button/Kconfig new file mode 100644 index 000000000000..e891c6f96c5e --- /dev/null +++ b/src/drivers/safety_button/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_SAFETY_BUTTON + bool "safety_button" + default n + ---help--- + Enable support for safety_button \ No newline at end of file diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp index 8551a37c1a84..18034b89cfc8 100644 --- a/src/drivers/safety_button/SafetyButton.cpp +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * 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 @@ -31,7 +31,6 @@ * ****************************************************************************/ -#include #include "SafetyButton.hpp" #ifndef GPIO_BTN_SAFETY @@ -54,11 +53,7 @@ enum class LED_PATTERN : uint16_t { SafetyButton::SafetyButton() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - _safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); - - if (_safety_disabled) { - _safety_btn_off = true; - } + _has_px4io = PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO); } SafetyButton::~SafetyButton() @@ -67,33 +62,22 @@ SafetyButton::~SafetyButton() } void -SafetyButton::CheckButton() +SafetyButton::CheckSafetyRequest(bool button_pressed) { - const bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); - - /* Keep safety button pressed for one second to turn off safety - * - * Note that safety cannot be turned on again by button because a button - * hardware problem could accidentally disable it in flight. - */ - if (safety_button_pressed && !_safety_btn_off) { + /* Keep button pressed for one second to turn off safety */ + if (button_pressed) { if (_button_counter <= CYCLE_COUNT) { _button_counter++; } if (_button_counter == CYCLE_COUNT) { - // switch safety off -> ready to arm state - _safety_btn_off = true; + _button_publisher.safetyButtonTriggerEvent(); } } else { _button_counter = 0; } - - CheckPairingRequest(safety_button_pressed); - - _safety_btn_prev_sate = safety_button_pressed; } void @@ -108,7 +92,7 @@ SafetyButton::CheckPairingRequest(bool button_pressed) _pairing_button_counter = 0; } - if (!_safety_btn_prev_sate && button_pressed) { + if (!_button_prev_sate && button_pressed) { if (_pairing_start == 0) { _pairing_start = now; } @@ -116,29 +100,8 @@ SafetyButton::CheckPairingRequest(bool button_pressed) ++_pairing_button_counter; } - if (_pairing_button_counter == 3) { - vehicle_command_s vcmd{}; - vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR; - vcmd.param1 = 10.f; // GCS pairing request handled by a companion. - vcmd.timestamp = hrt_absolute_time(); - _to_command.publish(vcmd); - PX4_DEBUG("Sending GCS pairing request"); - - led_control_s led_control{}; - led_control.led_mask = 0xff; - led_control.mode = led_control_s::MODE_BLINK_FAST; - led_control.color = led_control_s::COLOR_GREEN; - led_control.num_blinks = 1; - led_control.priority = 0; - led_control.timestamp = hrt_absolute_time(); - _to_led_control.publish(led_control); - - tune_control_s tune_control{}; - tune_control.tune_id = tune_control_s::TUNE_ID_NOTIFY_POSITIVE; - tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - tune_control.timestamp = hrt_absolute_time(); - _to_tune_control.publish(tune_control); - + if (_pairing_button_counter == ButtonPublisher::PAIRING_BUTTON_EVENT_COUNT) { + _button_publisher.pairingButtonTriggerEvent(); // reset state _pairing_start = 0; _pairing_button_counter = 0; @@ -156,7 +119,7 @@ SafetyButton::FlashButton() LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM; // cycle the blink state machine - if (_safety_btn_off) { + if (_button_prev_sate) { if (armed.armed) { pattern = LED_PATTERN::IO_FMU_ARMED; @@ -190,25 +153,16 @@ SafetyButton::Run() return; } - CheckButton(); + const bool button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); - // control safety switch LED & publish safety topic - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + // control safety switch LED & safety button + if (!_has_px4io) { FlashButton(); - - const bool safety_off = _safety_btn_off || _safety_disabled; - - // publish immediately on change, otherwise at 1 Hz - if ((hrt_elapsed_time(&_safety.timestamp) >= 1_s) - || (_safety.safety_off != safety_off)) { - - _safety.safety_switch_available = true; - _safety.safety_off = safety_off; - _safety.timestamp = hrt_absolute_time(); - - _to_safety.publish(_safety); - } + CheckSafetyRequest(button_pressed); } + + CheckPairingRequest(button_pressed); + _button_prev_sate = button_pressed; } int @@ -248,15 +202,6 @@ SafetyButton::custom_command(int argc, char *argv[]) return print_usage("unknown command"); } -int -SafetyButton::print_status() -{ - PX4_INFO("Safety Disabled: %s", _safety_disabled ? "yes" : "no"); - PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on"); - - return 0; -} - int SafetyButton::print_usage(const char *reason) { diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 046c7ee9c61a..0f38b6ba821a 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * 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 @@ -35,20 +35,10 @@ #include -#include #include -#include -#include -#include #include #include -#include -#include -#include -#include -#include -#include -#include +#include