diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests index a12d0f304679..8922bd1bb32a 100644 --- a/.ci/Jenkinsfile-SITL_tests +++ b/.ci/Jenkinsfile-SITL_tests @@ -8,7 +8,7 @@ pipeline { stage('Build') { agent { docker { - image 'px4io/px4-dev-ros-kinetic:2019-03-08' + image 'px4io/px4-dev-ros-kinetic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -93,6 +93,13 @@ pipeline { vehicle: "iris_obs_avoid", run_script: "rostest_avoidance_run.sh" ], + [ + name: "MC_safe_landing", + test: "mavros_posix_test_safe_landing.test", + mission: "MC_safe_landing", + vehicle: "iris_obs_avoid", + run_script: "rostest_avoidance_run.sh" + ], ] @@ -123,7 +130,7 @@ def createTestNode(Map test_def) { return { node { cleanWs() - docker.image("px4io/px4-dev-ros-kinetic:2019-03-08").inside('-e HOME=${WORKSPACE}') { + docker.image("px4io/px4-dev-ros-kinetic:2019-07-29").inside('-e HOME=${WORKSPACE}') { stage(test_def.name) { def run_script = test_def.get('run_script', 'rostest_px4_run.sh') def test_ok = true diff --git a/.ci/Jenkinsfile-SITL_tests_ASan b/.ci/Jenkinsfile-SITL_tests_ASan index 7b759b26b5d3..28e068fc3411 100644 --- a/.ci/Jenkinsfile-SITL_tests_ASan +++ b/.ci/Jenkinsfile-SITL_tests_ASan @@ -8,7 +8,7 @@ pipeline { stage('Build') { agent { docker { - image 'px4io/px4-dev-ros-kinetic:2019-03-08' + image 'px4io/px4-dev-ros-kinetic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -117,7 +117,7 @@ def createTestNode(Map test_def) { return { node { cleanWs() - docker.image("px4io/px4-dev-ros-kinetic:2019-03-08").inside('-e HOME=${WORKSPACE}') { + docker.image("px4io/px4-dev-ros-kinetic:2019-07-29").inside('-e HOME=${WORKSPACE}') { stage(test_def.name) { def test_ok = true sh('export') diff --git a/.ci/Jenkinsfile-SITL_tests_coverage b/.ci/Jenkinsfile-SITL_tests_coverage index 0533f6bfc6c7..525fb4f02f20 100644 --- a/.ci/Jenkinsfile-SITL_tests_coverage +++ b/.ci/Jenkinsfile-SITL_tests_coverage @@ -79,7 +79,7 @@ pipeline { stage('code coverage (python)') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-base-bionic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -99,7 +99,7 @@ pipeline { stage('unit tests') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-base-bionic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -137,7 +137,7 @@ def createTestNode(Map test_def) { return { node { cleanWs() - docker.image("px4io/px4-dev-ros-kinetic:2019-03-08").inside('-e HOME=${WORKSPACE}') { + docker.image("px4io/px4-dev-ros-kinetic:2019-07-29").inside('-e HOME=${WORKSPACE}') { stage(test_def.name) { def test_ok = true sh('export') diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 119eeefbaebf..bd99be226362 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:2019-03-08", - base: "px4io/px4-dev-base-bionic:2019-03-08", - nuttx: "px4io/px4-dev-nuttx:2019-03-08", - ros: "px4io/px4-dev-ros-kinetic:2019-03-08", - rpi: "px4io/px4-dev-raspi:2019-03-08", + armhf: "px4io/px4-dev-armhf:2019-07-29", + base: "px4io/px4-dev-base-bionic:2019-07-29", + nuttx: "px4io/px4-dev-nuttx:2019-07-29", + rpi: "px4io/px4-dev-raspi:2019-07-29", snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12" ] @@ -36,9 +35,9 @@ pipeline { "px4_fmu-v4_default", "px4_fmu-v4pro_default", "px4_fmu-v5_default", "px4_fmu-v5_fixedwing", "px4_fmu-v5_multicopter", "px4_fmu-v5_rover", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", - "px4_fmu-v5x_default", "px4_fmu-v5x_fixedwing", "px4_fmu-v5x_multicopter", "px4_fmu-v5x_rover", "px4_fmu-v5x_rtps", "px4_fmu-v5x_stackcheck", + "px4_fmu-v5x_default", "intel_aerofc-v1_default", "auav_x21_default", "av_x-v1_default", "bitcraze_crazyflie_default", "airmind_mindpx-v2_default", - "nxp_fmuk66-v3_default", "omnibus_f4sd_default", "holybro_kakutef7"], + "holybro_kakutef7", "mro_ctrl-zero-f7_default", "nxp_fmuk66-v3_default", "omnibus_f4sd_default", "uvify_core_default"], image: docker_images.nuttx, archive: true ] @@ -82,7 +81,7 @@ pipeline { // TODO: actually upload artifacts to S3 // stage('S3 Upload') { // agent { - // docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + // docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } // } // options { // skipDefaultCheckout() diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index ce33b34691d2..66f004dccade 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -11,7 +11,7 @@ pipeline { stage('px4_fmu-v2_test') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -35,7 +35,7 @@ pipeline { stage('px4_fmu-v3_default') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -59,7 +59,7 @@ pipeline { stage('px4_fmu-v4_default') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -83,7 +83,7 @@ pipeline { stage('px4_fmu-v4pro_default') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -107,7 +107,7 @@ pipeline { stage('px4_fmu-v5_default') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -128,10 +128,58 @@ pipeline { } } + stage('px4_fmu-v5_critmonitor') { + agent { + docker { + image 'px4io/px4-dev-nuttx:2019-07-29' + args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + } + } + steps { + sh 'export' + sh 'make distclean' + sh 'ccache -z' + sh 'git fetch --tags' + sh 'make px4_fmu-v5_critmonitor' + sh 'make sizes' + sh 'ccache -s' + stash includes: 'build/px4_fmu-v5_critmonitor/px4_fmu-v5_critmonitor.elf', name: 'px4_fmu-v5_critmonitor' + } + post { + always { + sh 'make distclean' + } + } + } + + stage('px4_fmu-v5_irqmonitor') { + agent { + docker { + image 'px4io/px4-dev-nuttx:2019-07-29' + args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + } + } + steps { + sh 'export' + sh 'make distclean' + sh 'ccache -z' + sh 'git fetch --tags' + sh 'make px4_fmu-v5_irqmonitor' + sh 'make sizes' + sh 'ccache -s' + stash includes: 'build/px4_fmu-v5_irqmonitor/px4_fmu-v5_irqmonitor.elf', name: 'px4_fmu-v5_irqmonitor' + } + post { + always { + sh 'make distclean' + } + } + } + stage('px4_fmu-v5_stackcheck') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -152,10 +200,34 @@ pipeline { } } + stage('px4_fmu-v5x_default') { + agent { + docker { + image 'px4io/px4-dev-nuttx:2019-07-29' + args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' + } + } + steps { + sh 'export' + sh 'make distclean' + sh 'ccache -z' + sh 'git fetch --tags' + sh 'make px4_fmu-v5x_default' + sh 'make sizes' + sh 'ccache -s' + stash includes: 'build/px4_fmu-v5x_default/px4_fmu-v5x_default.elf', name: 'px4_fmu-v5x_default' + } + post { + always { + sh 'make distclean' + } + } + } + stage('nxp_fmuk66-v3_default') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -187,41 +259,17 @@ pipeline { agent { label 'px4_fmu-v2' } - steps { - script { - try { - sh 'export' - sh 'find /dev/serial' - unstash 'px4_fmu-v2_test' - sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v2_test/px4_fmu-v2_test.elf' - sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' - //sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' - // ensure buzzer is disabled for the next test - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' - } catch (Exception err) { - // always report passed for now - currentBuild.result = 'SUCCESS' - } - } // script - } - options { - timeout(time: 20, unit: 'MINUTES') - } - } - - stage('px4_fmu-v3_default') { - agent { - label 'px4_fmu-v3' - } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v3_default' + unstash 'px4_fmu-v2_test' // flash board and watch bootup - sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v3_default/px4_fmu-v3_default.elf' + sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v2_test/px4_fmu-v2_test.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' @@ -236,79 +284,119 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' - // run tests - sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' + // ensure buzzer is disabled for the next test + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v4_default') { + stage('px4_fmu-v3_default') { agent { - label 'px4_fmu-v4' + label 'px4_fmu-v3' } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v4_default' + unstash 'px4_fmu-v3_default' // flash board and watch bootup - sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4_default/px4_fmu-v4_default.elf' + sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v3_default/px4_fmu-v3_default.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v4pro_default') { + stage('px4_fmu-v4_default') { agent { - label 'px4_fmu-v4pro' + label 'px4_fmu-v4' } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v4pro_default' + unstash 'px4_fmu-v4_default' // flash board and watch bootup - sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4pro_default/px4_fmu-v4pro_default.elf' + sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4_default/px4_fmu-v4_default.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' @@ -316,64 +404,100 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v5_default (pixhawk 4)') { + stage('px4_fmu-v4pro_default') { agent { - label 'pixhawk_4' + label 'px4_fmu-v4pro' } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v5_default' + unstash 'px4_fmu-v4pro_default' // flash board and watch bootup - sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_default/px4_fmu-v5_default.elf' + sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4pro_default/px4_fmu-v4pro_default.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v5_default (pixhawk 4 mini)') { + stage('px4_fmu-v5_default') { agent { - label 'pixhawk_4_mini' + label 'px4_fmu-v5' } steps { sh 'export' @@ -382,112 +506,200 @@ pipeline { // flash board and watch bootup sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_default/px4_fmu-v5_default.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v5_default (CUAV V5 Nano)') { + stage('px4_fmu-v5_critmonitor') { agent { - label 'cuav_v5_nano' + label 'px4_fmu-v5' } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v5_default' + unstash 'px4_fmu-v5_critmonitor' // flash board and watch bootup - sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_default/px4_fmu-v5_default.elf' + sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_critmonitor/px4_fmu-v5_critmonitor.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + // critmon + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "critmon_start; sleep 5; critmon_stop"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' + // critmon + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "critmon_start; sleep 5; critmon_stop"' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } options { timeout(time: 20, unit: 'MINUTES') } } - stage('px4_fmu-v5_default (CUAV V5+)') { + stage('px4_fmu-v5_irqmonitor') { agent { - label 'cuav_v5_plus' + label 'px4_fmu-v5' } steps { sh 'export' sh 'find /dev/serial' - unstash 'px4_fmu-v5_default' + unstash 'px4_fmu-v5_irqmonitor' // flash board and watch bootup - sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_default/px4_fmu-v5_default.elf' + sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_irqmonitor/px4_fmu-v5_irqmonitor.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // irqinfo + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "irqinfo"' + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + // irqinfo + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "irqinfo"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' + // irqinfo + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "irqinfo"' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } options { timeout(time: 20, unit: 'MINUTES') @@ -504,9 +716,46 @@ pipeline { sh 'export' sh 'find /dev/serial' unstash 'px4_fmu-v5_stackcheck' + // flash board and watch bootup sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info -d /dev/pwm_output1"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' + // run tests // sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // TODO: currently unusably slow + // ensure buzzer is disabled for the next test + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "px4io stop"' } catch (Exception err) { // always report passed for now currentBuild.result = 'SUCCESS' @@ -518,6 +767,73 @@ pipeline { } } + stage('px4_fmu-v5x_default') { + agent { + label 'px4_fmu-v5x' + } + steps { + sh 'export' + sh 'find /dev/serial' + unstash 'px4_fmu-v5x_default' + // flash board and watch bootup + sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v5x_default/px4_fmu-v5x_default.elf' + sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-SEGGER_*` --baudrate 57600' + // configure + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param set SYS_AUTOSTART 13000"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger on"' + // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "fmu status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /etc"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ls /proc"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink status streams"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mtd status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param show"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "perf latency"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "perf"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ps"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "pwm info -d /dev/pwm_output1"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "px4io status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sd_bench -r 2"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "sensors status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "top once"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"' + // run tests + sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-SEGGER_*`' + // ensure buzzer is disabled for the next test + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "fmu stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "px4io stop"' + } + options { + timeout(time: 20, unit: 'MINUTES') + } + } + stage('nxp_fmuk66-v3_default') { agent { label 'nxp_fmuk66-v3' @@ -529,14 +845,28 @@ pipeline { // flash board and watch bootup sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/nxp_fmuk66-v3_default/nxp_fmuk66-v3_default.elf' sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600' + // configure + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set SYS_AUTOSTART 4001"' // generic multicopter + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "reboot"' // reboot to apply + // run logger + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger on"' // status commands + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "adc test"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander check"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /etc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /proc"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status streams"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"' @@ -544,14 +874,24 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "pwm info"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sd_bench -r 2"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"' // run tests sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // ensure buzzer is disabled for the next test sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' + // manually stop everything + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink stop-all"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ekf2 stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "navigator stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "logger stop"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "fmu stop"' } options { timeout(time: 20, unit: 'MINUTES') diff --git a/.ci/Jenkinsfile-hardware_linux b/.ci/Jenkinsfile-hardware_linux new file mode 100644 index 000000000000..d884183aa333 --- /dev/null +++ b/.ci/Jenkinsfile-hardware_linux @@ -0,0 +1,41 @@ +#!/usr/bin/env groovy + +pipeline { + agent none + stages { + stage('Build and Verify') { + + agent { + label 'navio2' + } + steps { + sh 'export' + sh 'make distclean' + sh 'ccache -z' + sh 'git fetch --tags' + sh 'CCACHE_BASEDIR=${WORKSPACE} make emlid_navio2_native' + sh 'ccache -s' + // sanity check + sh 'cd build/emlid_navio2_native/ && ./bin/px4 -s ../../posix-configs/rpi/px4_test.config' + } + post { + always { + sh 'make distclean' + } + } + options { + timeout(time: 30, unit: 'MINUTES') + } + + } + + } // stages + environment { + CCACHE_DIR = '/tmp/ccache' + CI = true + } + options { + buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '30')) + timeout(time: 60, unit: 'MINUTES') + } +} diff --git a/.ci/Jenkinsfile-hardware_snapdragon b/.ci/Jenkinsfile-hardware_snapdragon new file mode 100644 index 000000000000..b5b042dae41b --- /dev/null +++ b/.ci/Jenkinsfile-hardware_snapdragon @@ -0,0 +1,48 @@ +#!/usr/bin/env groovy + +pipeline { + agent none + stages { + stage('Build and Verify') { + + agent { + label 'snapdragon' + } + steps { + sh 'export' + sh 'make distclean' + sh 'ccache -z' + sh 'git fetch --tags' + sh 'CCACHE_BASEDIR=${WORKSPACE} make eagle_default' + sh 'ccache -s' + // sanity check + sh 'adb devices' + sh 'make eagle_default sanity || true' + } + post { + always { + sh 'cat boards/atlflight/eagle/scripts/px4.log' + sh 'cat boards/atlflight/eagle/scripts/minidm.log' + sh 'make distclean' + } + } + options { + timeout(time: 30, unit: 'MINUTES') + } + + } + + } // stages + environment { + CCACHE_DIR = '/tmp/ccache' + CI = true + ARM_CROSS_GCC_ROOT="/home/jenkins/Qualcomm/ARM_Tools/gcc-4.9-2014.11" + HEXAGON_ARM_SYSROOT="/home/jenkins/Qualcomm/qrlinux_sysroot" + HEXAGON_SDK_ROOT="/home/jenkins/Qualcomm/Hexagon_SDK/3.0" + HEXAGON_TOOLS_ROOT="/home/jenkins/Qualcomm/HEXAGON_Tools/7.2.12/Tools" + } + options { + buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '30')) + timeout(time: 60, unit: 'MINUTES') + } +} diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 000000000000..e21b6691985a --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,19 @@ +version: 2 +jobs: + build: + docker: + - image: px4io/px4-dev-nuttx:2019-07-29 + steps: + - checkout + - run: + name: Fetch tags + command: git fetch --tags + - run: + name: Build px4_fmu-v5_default + command: make px4_fmu-v5_default + - store_artifacts: + path: build/px4_fmu-v5_default/px4_fmu-v5_default.px4 + destination: px4_fmu-v5_default.px4 + - store_artifacts: + path: build/px4_fmu-v5_default/px4_fmu-v5_default.elf + destination: px4_fmu-v5_default.elf diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 000000000000..ee07a7a2b6e2 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,7 @@ +* text=auto eol=lf +*.{cmd,[cC][mM][dD]} text eol=crlf +*.{bat,[bB][aA][tT]} text eol=crlf + +*.bin binary +*.pdf binary +*.png binary diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml new file mode 100644 index 000000000000..c900e46b4605 --- /dev/null +++ b/.github/workflows/ccpp.yml @@ -0,0 +1,13 @@ +name: C/C++ CI + +on: [push] + +jobs: + build: + runs-on: ubuntu-latest + container: px4io/px4-dev-base-bionic:2019-07-29 + steps: + - uses: actions/checkout@v1 + - name: make + run: make + diff --git a/.gitignore b/.gitignore index d4d5a9d446b1..ad24099d5121 100644 --- a/.gitignore +++ b/.gitignore @@ -17,7 +17,6 @@ Testing/ Packages/* build/* build_*/ -core cscope.out cscope.in.out cscope.po.out @@ -64,3 +63,7 @@ posix-configs/SITL/init/test/*_generated *.gcov .coverage .coverage.* + +# KDevelop ignores +.kdev4/* +*.kdev4 diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 5603d16a9850..11aac057e264 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -61,6 +61,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: intel_aerofc-v1_default + mro_ctrl-zero-f7_default: + short: mro_ctrl-zero-f7 + buildType: MinSizeRel + settings: + CONFIG: mro_ctrl-zero-f7_default nxp_fmuk66-v3_default: short: nxp_fmuk66-v3 buildType: MinSizeRel diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 163f1b1c9f8e..68d2edcbef8d 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -2,14 +2,14 @@ // See http://go.microsoft.com/fwlink/?LinkId=827846 // for the documentation about the extensions.json format "recommendations": [ - "ajshort.ros", "chiehyu.vscode-astyle", "dan-c-underwood.arm", "github.vscode-pull-request-github", "marus25.cortex-debug", + "ms-azuretools.vscode-docker", + "ms-iot.vscode-ros", "ms-python.python", "ms-vscode.cpptools", - "peterjausovec.vscode-docker", "twxs.cmake", "uavcan.dsdl", "vector-of-bool.cmake-tools", diff --git a/.vscode/launch.json b/.vscode/launch.json index 474e68f5b3c7..b87dd7f4a273 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -220,7 +220,7 @@ ] }, { - "name": "Debug av_x-v1 (jlink)", + "name": "Debug av_x-v1/mro_ctrl-zero-f7 (jlink)", "device": "STM32F777NI", "svdFile": "${workspaceRoot}/../cmsis-svd/data/STMicro/STM32F7x7.svd", "executable": "${command:cmake.launchTargetPath}", diff --git a/.vscode/settings.json b/.vscode/settings.json index a6d229884c82..0ed788738023 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -53,6 +53,7 @@ ] } }, + "cortex-debug.enableTelemetry": false, "C_Cpp.clang_format_fallbackStyle": "none", "C_Cpp.configurationWarnings": "Disabled", "C_Cpp.default.cppStandard": "c++11", @@ -63,6 +64,8 @@ "C_Cpp.intelliSenseEngine": "Default", "C_Cpp.intelliSenseEngineFallback": "Disabled", "debug.toolBarLocation": "docked", + "editor.defaultFormatter": "chiehyu.vscode-astyle", + "editor.dragAndDrop": false, "editor.insertSpaces": false, "editor.minimap.maxColumn": 120, "editor.minimap.renderCharacters": false, @@ -74,7 +77,14 @@ "explorer.openEditors.visible": 0, "files.insertFinalNewline": true, "files.trimTrailingWhitespace": true, + "files.watcherExclude": { + "**/build/*": true + }, "git.detectSubmodulesLimit": 20, + "git.ignoreLimitWarning": true, + "githubPullRequests.defaultMergeMethod": "squash", + "githubPullRequests.telemetry.enabled": false, + "gitlens.advanced.telemetry.enabled": false, "files.associations": { "*.jinja": "jinja" }, @@ -82,9 +92,10 @@ "build/**": true }, "search.showLineNumbers": true, + "telemetry.enableTelemetry": false, "window.title": "${dirty} ${activeEditorMedium}${separator}${rootName}", "workbench.editor.highlightModifiedTabs": true, + "workbench.enableExperiments": false, "workbench.settings.enableNaturalLanguageSearch": false, - "workbench.statusBar.feedback.visible": false, - "git.ignoreLimitWarning": true + "workbench.statusBar.feedback.visible": false } diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ee7b3bb27f2..a3071a9455fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -347,6 +347,17 @@ else() px4_find_python_module(jinja2 REQUIRED) endif() +#============================================================================= +# get chip and chip manufacturer +# +px4_os_determine_build_chip() +if(NOT PX4_CHIP_MANUFACTURER) + message(FATAL_ERROR "px4_os_determine_build_chip() needs to set PX4_CHIP_MANUFACTURER") +endif() +if(NOT PX4_CHIP) + message(FATAL_ERROR "px4_os_determine_build_chip() needs to set PX4_CHIP") +endif() + #============================================================================= # build flags # @@ -451,7 +462,8 @@ add_library(parameters_interface INTERFACE) include(px4_add_library) add_subdirectory(src/lib EXCLUDE_FROM_ALL) -add_subdirectory(src/platforms EXCLUDE_FROM_ALL) +add_subdirectory(platforms/${PX4_PLATFORM}/src/px4) +add_subdirectory(platforms EXCLUDE_FROM_ALL) add_subdirectory(src/modules/uORB EXCLUDE_FROM_ALL) # TODO: platform layer add_subdirectory(src/drivers/boards EXCLUDE_FROM_ALL) @@ -598,3 +610,4 @@ else() endif() include(CPack) +include(bloaty) diff --git a/Jenkinsfile b/Jenkinsfile index 2bebf888a4eb..bf8488211eba 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -11,7 +11,7 @@ pipeline { stage('Catkin build on ROS workspace') { agent { docker { - image 'px4io/px4-dev-ros-melodic:2019-03-08' + image 'px4io/px4-dev-ros-melodic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -51,7 +51,7 @@ pipeline { stage('Colcon build on ROS2 workspace') { agent { docker { - image 'px4io/px4-dev-ros2-bouncy:2019-03-08' + image 'px4io/px4-dev-ros2-bouncy:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -82,7 +82,7 @@ pipeline { stage('Style check') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh 'make check_format' @@ -94,10 +94,10 @@ pipeline { } } - stage('Bloaty px4_fmu-v2') { + stage('px4_fmu-v2 (bloaty)') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -107,9 +107,11 @@ pipeline { sh 'ccache -z' sh 'git fetch --tags' sh 'make px4_fmu-v2_default' - sh 'make px4_fmu-v2_default bloaty_symbols' sh 'make px4_fmu-v2_default bloaty_compileunits' sh 'make px4_fmu-v2_default bloaty_inlines' + sh 'make px4_fmu-v2_default bloaty_sections' + sh 'make px4_fmu-v2_default bloaty_segments' + sh 'make px4_fmu-v2_default bloaty_symbols' sh 'make px4_fmu-v2_default bloaty_templates' sh 'make px4_fmu-v2_default bloaty_compare_master' sh 'make sizes' @@ -120,12 +122,15 @@ pipeline { sh 'make distclean' } } + environment { + CCACHE_DISABLE = 1 + } } - stage('Bloaty px4_fmu-v5') { + stage('px4_fmu-v5 (bloaty)') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -135,9 +140,11 @@ pipeline { sh 'ccache -z' sh 'git fetch --tags' sh 'make px4_fmu-v5_default' - sh 'make px4_fmu-v5_default bloaty_symbols' sh 'make px4_fmu-v5_default bloaty_compileunits' sh 'make px4_fmu-v5_default bloaty_inlines' + sh 'make px4_fmu-v5_default bloaty_sections' + sh 'make px4_fmu-v5_default bloaty_segments' + sh 'make px4_fmu-v5_default bloaty_symbols' sh 'make px4_fmu-v5_default bloaty_templates' sh 'make px4_fmu-v5_default bloaty_compare_master' sh 'make sizes' @@ -148,12 +155,15 @@ pipeline { sh 'make distclean' } } + environment { + CCACHE_DISABLE = 1 + } } - stage('No-ninja px4_fmu-v2') { + stage('px4_sitl (bloaty)') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -162,7 +172,15 @@ pipeline { sh 'make distclean' sh 'ccache -z' sh 'git fetch --tags' - sh 'NO_NINJA_BUILD=1 make px4_fmu-v2_default' + sh 'make px4_sitl_default' + sh 'make px4_sitl_default bloaty_compileunits' + sh 'make px4_sitl_default bloaty_inlines' + sh 'make px4_sitl_default bloaty_sections' + sh 'make px4_sitl_default bloaty_segments' + sh 'make px4_sitl_default bloaty_symbols' + sh 'make px4_sitl_default bloaty_templates' + //sh 'make px4_fmu-v5_default bloaty_compare_master' + sh 'make sizes' sh 'ccache -s' } post { @@ -170,12 +188,15 @@ pipeline { sh 'make distclean' } } + environment { + CCACHE_DISABLE = 1 + } } - stage('No-ninja px4_fmu-v5') { + stage('px4_fmu-v5 (no ninja)') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -184,7 +205,8 @@ pipeline { sh 'make distclean' sh 'ccache -z' sh 'git fetch --tags' - sh 'NO_NINJA_BUILD=1 make px4_fmu-v5_default' + sh 'make px4_fmu-v5_default' + sh 'make sizes' sh 'ccache -s' } post { @@ -192,12 +214,15 @@ pipeline { sh 'make distclean' } } + environment { + NO_NINJA_BUILD = 1 + } } - stage('No-ninja SITL build') { + stage('px4_sitl (no ninja)') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -206,7 +231,8 @@ pipeline { sh 'make distclean' sh 'ccache -z' sh 'git fetch --tags' - sh 'NO_NINJA_BUILD=1 make px4_sitl_default' + sh 'make px4_sitl_default' + sh 'make sizes' sh 'ccache -s' } post { @@ -214,12 +240,15 @@ pipeline { sh 'make distclean' } } + environment { + NO_NINJA_BUILD = 1 + } } stage('SITL unit tests') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-base-bionic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -241,7 +270,7 @@ pipeline { stage('Clang analyzer') { agent { docker { - image 'px4io/px4-dev-clang:2019-03-08' + image 'px4io/px4-dev-clang:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -279,7 +308,7 @@ pipeline { // stage('Clang tidy') { // agent { // docker { - // image 'px4io/px4-dev-clang:2019-03-08' + // image 'px4io/px4-dev-clang:2019-07-29' // args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' // } // } @@ -300,7 +329,7 @@ pipeline { stage('Cppcheck') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-base-bionic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -338,7 +367,7 @@ pipeline { stage('Check stack') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -358,7 +387,7 @@ pipeline { stage('ShellCheck') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -377,7 +406,7 @@ pipeline { stage('Module config validation') { agent { docker { - image 'px4io/px4-dev-base-bionic:2019-03-08' + image 'px4io/px4-dev-base-bionic:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -402,7 +431,7 @@ pipeline { stage('Airframe') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh 'make distclean' @@ -421,7 +450,7 @@ pipeline { stage('Parameter') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh 'make distclean' @@ -440,7 +469,7 @@ pipeline { stage('Module') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh 'make distclean' @@ -460,7 +489,7 @@ pipeline { stage('uORB graphs') { agent { docker { - image 'px4io/px4-dev-nuttx:2019-03-08' + image 'px4io/px4-dev-nuttx:2019-07-29' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -489,7 +518,7 @@ pipeline { stage('Devguide') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') @@ -519,7 +548,7 @@ pipeline { stage('Userguide') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') @@ -547,7 +576,7 @@ pipeline { stage('QGroundControl') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') @@ -575,7 +604,7 @@ pipeline { stage('PX4 ROS msgs') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') @@ -604,7 +633,7 @@ pipeline { stage('PX4 ROS2 bridge') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') @@ -635,7 +664,7 @@ pipeline { stage('S3') { agent { - docker { image 'px4io/px4-dev-base-bionic:2019-03-08' } + docker { image 'px4io/px4-dev-base-bionic:2019-07-29' } } steps { sh('export') diff --git a/Makefile b/Makefile index a708a1a97774..c72b8f817fb7 100644 --- a/Makefile +++ b/Makefile @@ -380,6 +380,7 @@ tests_offboard: rostest tests_avoidance: rostest @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test + @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test python_coverage: @mkdir -p "$(SRC_DIR)"/build/python_coverage diff --git a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow index a7c9b289633b..595c06b04508 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/1010_iris_opt_flow @@ -14,13 +14,6 @@ then param set EKF2_EVP_NOISE 0.05 param set EKF2_EVA_NOISE 0.05 - # INAV - param set INAV_LIDAR_EST 1 - param set INAV_W_XY_FLOW 1 - param set INAV_W_XY_GPS_P 0 - param set INAV_W_XY_GPS_V 0 - param set INAV_W_Z_GPS_P 0 - # LPE: Flow-only mode param set LPE_FUSION 242 param set LPE_FAKE_ORIGIN 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision index b20e1886bec7..7bf135d7928b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/1013_iris_vision @@ -13,13 +13,6 @@ then param set EKF2_AID_MASK 24 param set EKF2_EV_DELAY 5 - # INAV: trust more on the vision input - param set INAV_W_XY_VIS_P 9 - param set INAV_W_Z_VIS_P 7 - param set INAV_W_XY_GPS_P 0 - param set INAV_W_XY_GPS_V 0 - param set INAV_W_Z_GPS_P 0 - # LPE: Vision + baro param set LPE_FUSION 132 diff --git a/ROMFS/px4fmu_common/init.d-posix/1030_plane b/ROMFS/px4fmu_common/init.d-posix/1030_plane index 6a149b36b6bc..f847687289ee 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1030_plane +++ b/ROMFS/px4fmu_common/init.d-posix/1030_plane @@ -37,7 +37,6 @@ then param set RWTO_TKOFF 1 - param set WEST_EN 1 fi set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix diff --git a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol index 24a5eff04e25..b906754dd148 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/1040_standard_vtol @@ -35,10 +35,10 @@ then param set VT_F_TRANS_DUR 5 param set VT_F_TRANS_THR 0.75 param set VT_ARSP_TRANS 16 - param set VT_MOT_COUNT 4 + param set VT_MOT_ID 1234 + param set VT_FW_MOT_OFFID 1234 param set VT_TYPE 2 - param set WEST_EN 1 fi set MAV_TYPE 22 diff --git a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter index dbcccec7035f..3c6856788d52 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/1041_tailsitter @@ -35,10 +35,8 @@ then param set VT_F_TRANS_DUR 1.5 param set VT_F_TRANS_THR 0.7 - param set VT_MOT_COUNT 0 param set VT_TYPE 0 - param set WEST_EN 1 fi set MAV_TYPE 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor index 2dc2c7271b82..3749ec2c033d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/1042_tiltrotor @@ -35,13 +35,11 @@ then param set VT_F_TRANS_DUR 1.5 param set VT_F_TRANS_THR 0.75 - param set VT_MOT_COUNT 4 param set VT_TILT_FW 3.1415 param set VT_TILT_TRANS 1.2 param set VT_ELEV_MC_LOCK 0 param set VT_TYPE 1 - param set WEST_EN 1 fi set MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index d0b3e248c188..a1c629c2764f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -162,7 +162,8 @@ then param set SENS_DPRES_OFF 0.001 param set SYS_RESTART_TYPE 2 - param set WEST_EN 0 + param set TRIG_INTERFACE 3 + fi # Adapt timeout parameters if simulation runs faster or slower than realtime. @@ -211,10 +212,6 @@ sensors start commander start navigator start -if param compare WEST_EN 1 -then - wind_estimator start -fi if ! param compare MNT_MODE_IN -1 then 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 62b674af2405..0f5a6c24f429 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -58,7 +58,8 @@ then param set SYS_RESTART_TYPE 2 param set VT_F_TRANS_THR 0.75 - param set VT_MOT_COUNT 4 + param set VT_MOT_ID 1234 + param set VT_FW_MOT_OFFID 1234 param set VT_TYPE 2 fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol index f6d8ff30f2ff..4542638645bd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol @@ -17,16 +17,20 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF = yes ] then + param set MAV_TYPE 19 + param set MC_ROLL_P 6 param set MC_ROLLRATE_P 0.12 param set MC_ROLLRATE_I 0.002 param set MC_ROLLRATE_D 0.003 param set MC_ROLLRATE_FF 0 + param set MC_PITCH_P 4.5 param set MC_PITCHRATE_P 0.3 param set MC_PITCHRATE_I 0.002 param set MC_PITCHRATE_D 0.003 param set MC_PITCHRATE_FF 0 + param set MC_YAW_P 3.8 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 @@ -37,10 +41,13 @@ then param set VT_ELEV_MC_LOCK 0 param set VT_MOT_ID 12 param set VT_TYPE 0 + + # Indicate that FW roll direction was fixed in mixer, to be removed in v1.10 + param set V19_VT_ROLLDIR 0 fi set MAV_TYPE 19 -set MIXER caipirinha_vtol +set MIXER vtol_tailsitter_duo set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 index 4e31e097dbd6..6fbae0d29a4a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 @@ -42,7 +42,8 @@ then param set VT_FW_MOT_OFFID 34 param set VT_IDLE_PWM_MC 1080 - param set VT_MOT_COUNT 123456 + param set VT_MOT_ID 123456 + param set VT_FW_MOT_OFFID 56 param set VT_TILT_MC 0.08 param set VT_TILT_TRANS 0.5 param set VT_TILT_FW 0.9 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter index 7c52dc0b91cd..781eb5897c76 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter +++ b/ROMFS/px4fmu_common/init.d/airframes/13004_quad+_tailsitter @@ -26,7 +26,6 @@ then param set PWM_MAX 2000 param set PWM_RATE 400 - param set VT_MOT_COUNT 1234 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 0 param set VT_ELEV_MC_LOCK 1 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 8dfa6c85c36b..7e531ff0ae4d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad +++ b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad @@ -58,7 +58,7 @@ then param set VT_ARSP_BLEND 6 param set VT_ARSP_TRANS 12 param set VT_F_TRANS_THR 0.75 - param set VT_MOT_COUNT 1234 + param set VT_MOT_ID 1234 param set VT_FW_MOT_OFFID 1234 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 397990a12db5..579ca6bbb355 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -97,7 +97,6 @@ then param set MPC_XY_VEL_MAX 5 param set MPC_ACC_HOR_MAX 2 param set MPC_LAND_SPEED 1.2 - param set MPC_MAN_R_MAX 30 param set MPC_TILTMAX_LND 35 param set MPC_Z_VEL_MAX 1.5 param set MPC_Z_VEL_MAX_UP 1.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index 66110990cecd..12601b21e167 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -28,7 +28,6 @@ then param set FW_AIRSPD_MAX 30 param set FW_AIRSPD_MIN 19 param set FW_AIRSPD_TRIM 23 - param set FW_CLMBOUT_DIFF 0.1 param set FW_L1_R_SLEW_MAX 40 param set FW_LND_EARLYCFG 1 param set FW_MAN_P_MAX 30 @@ -39,7 +38,7 @@ then param set FW_RLL_TO_YAW_FF 0.1 param set FW_RR_I 0.1 param set FW_RR_P 0.08 - param set FW_R_LIM 35 + param set FW_R_LIM 45 param set FW_R_RMAX 50 param set FW_THR_CRUISE 0.65 param set FW_THR_MIN 0.3 @@ -63,22 +62,22 @@ then param set MC_YAWRATE_MAX 40 param set MC_YAWRATE_P 0.3 - param set MIS_TAKEOFF_ALT 20 - param set MIS_YAW_TMT 10 + param set MIS_TAKEOFF_ALT 30 param set MPC_ACC_DOWN_MAX 2 param set MPC_ACC_HOR_MAX 2 param set MPC_ACC_UP_MAX 3 + param set MC_AIRMODE 1 param set MPC_JERK_AUTO 8 param set MPC_JERK_AUTO 15 param set MPC_LAND_SPEED 1 param set MPC_MAN_TILT_MAX 25 param set MPC_MAN_Y_MAX 40 param set MPC_POS_MODE 3 - param set MPC_SPOOLUP_TIME 2 + param set MPC_SPOOLUP_TIME 1.5 param set MPC_THR_HOVER 0.45 param set MPC_TILTMAX_AIR 25 - param set MPC_TKO_RAMP_T 3 + param set MPC_TKO_RAMP_T 1.8 param set MPC_TKO_SPEED 1 param set MPC_VEL_MANUAL 3 param set MPC_XY_CRUISE 3 @@ -97,11 +96,11 @@ then param set VT_ARSP_BLEND 10 param set VT_ARSP_TRANS 21 + param set VT_B_DEC_MSS 1.5 param set VT_B_TRANS_DUR 12 param set VT_ELEV_MC_LOCK 0 param set VT_FWD_THRUST_SC 1.2 param set VT_FW_MOT_OFFID 5678 - param set VT_F_TRANS_THR 0.85 param set VT_F_TR_OL_TM 8 param set VT_IDLE_PWM_MC 1000 param set VT_MOT_ID 5678 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter new file mode 100644 index 000000000000..232a9bc5073e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter @@ -0,0 +1,30 @@ +#!/bin/sh +# +# @name Generic Tailsitter +# +# @type VTOL Duo Tailsitter +# @class VTOL +# +# @output MAIN1 motor right +# @output MAIN2 motor left +# @output MAIN5 elevon right +# @output MAIN6 elevon left +# +# @maintainer Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +if [ $AUTOCNF = yes ] +then + param set VT_ELEV_MC_LOCK 0 + param set VT_MOT_COUNT 2 + param set VT_TYPE 0 + + param set MAV_TYPE 19 +fi + +set MAV_TYPE 19 +set MIXER vtol_tailsitter_duo + +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 60605cc16db3..709e8197ef5a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/airframes/4001_quad_x @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic Quadrotor x +# @name Generic Quadcopter # # @type Quadrotor x # @class Copter diff --git a/ROMFS/px4fmu_common/init.d/airframes/4013_bebop b/ROMFS/px4fmu_common/init.d/airframes/4013_bebop index 6be8d107e854..b97ce76dc615 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4013_bebop +++ b/ROMFS/px4fmu_common/init.d/airframes/4013_bebop @@ -10,7 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Michael Schaeuble # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb index 36d4666ab130..9bc37f9f5f46 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_hk_micro_pcb @@ -10,7 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Thomas Gubler # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo index 1590b71abc71..cc36bb8d34fc 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo +++ b/ROMFS/px4fmu_common/init.d/airframes/4030_3dr_solo @@ -6,10 +6,12 @@ # @class Copter # # @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 aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Andreas Antener # @@ -25,11 +27,6 @@ then param set MPC_XY_VEL_MAX 3 param set MPC_Z_VEL_MAX_DN 2 - # INAV: higher GPS weights for better altitude control - param set INAV_W_Z_BARO 0.3 - param set INAV_W_Z_GPS_P 0.8 - param set INAV_W_Z_GPS_V 0.8 - # takeoff, land and RTL settings param set MIS_TAKEOFF_ALT 4 param set RTL_LAND_DELAY 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index 2f4949b49ad3..6b1f8abbf8e7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -10,6 +10,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @output MAIN1 motor 1 # @output MAIN2 motor 2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc index 341dd809f5fd..d7b0c1ab95e3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc +++ b/ROMFS/px4fmu_common/init.d/airframes/4070_aerofc @@ -7,6 +7,7 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude # # @type Quadrotor x # @class Copter diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo new file mode 100644 index 000000000000..994613ed5115 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -0,0 +1,123 @@ +#!/bin/sh +# +# @name UVify IFO +# +# @type Quadrotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# +# @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 intel_aerofc-v1 exclude +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER quad_x +set PWM_OUT 1234 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + #param set MC_ROLL_P 7.00000 + param set MC_ROLLRATE_P 0.15000 + #param set MC_ROLLRATE_I 0.90000 + param set MC_ROLLRATE_D 0.00130 + + #param set MC_PITCH_P 7.00000 + param set MC_PITCHRATE_P 0.15000 + #param set MC_PITCHRATE_I 1.10000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + #param set MC_YAWRATE_I 0.15 + param set MC_YAWRATE_D 0.0 + param set 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.00000 + #param set MC_PITCHRATE_MAX 1600.00000 + #param set MC_ROLLRATE_MAX 1600.00000 + #param set MC_YAWRATE_MAX 700.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + #param set MPC_MAN_TILT_MAX 35.0000 + #param set MPC_TILTMAX_AIR 20.0000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + #param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_RATE 0 + + #param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 10 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + param set MPC_XY_P 1.7000 + param set MPC_XY_VEL_P 0.1300 + param set MPC_XY_VEL_I 0.0600 + param set MPC_XY_VEL_D 0.0100 + param set MPC_TKO_RAMP_T 1.0000 + param set MPC_TKO_SPEED 1.1000 + param set MPC_VEL_MANUAL 3.0000 + + param set BAT_SOURCE 0 + param set BAT_N_CELLS 4 + param set BAT_V_DIV 10.14 + param set BAT_A_PER_V 18.18 + #param set CBRK_IO_SAFETY 22027 + param set COM_ARM_EKF_AB 0.00500 + param set COM_DISARM_LAND 2 + + # Filter settings + param set IMU_GYRO_CUTOFF 90.00000 + param set MC_DTERM_CUTOFF 70.00000 + + # 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 SDLOG_PROFILE 19 + + # TELEM1 ttyS1 - Wifi module + param set MAV_0_CONFIG 101 + param set MAV_0_MODE 2 # onboard + param set SER_TEL1_BAUD 921600 + + # TELEM2 ttyS2 - Sub 1-Ghz + param set MAV_1_CONFIG 102 + param set MAV_1_MODE 0 # normal + param set SER_TEL2_BAUD 57600 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4072_draco b/ROMFS/px4fmu_common/init.d/airframes/4072_draco new file mode 100644 index 000000000000..661572074d82 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4072_draco @@ -0,0 +1,119 @@ +#!/bin/sh +# +# @name UVify Draco +# +# @type Quadrotor x +# @class Copter +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# +# @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 intel_aerofc-v1 exclude +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER quad_x +set PWM_OUT 1234 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + param set MC_ROLL_P 7.00000 + param set MC_ROLLRATE_P 0.15000 + param set MC_ROLLRATE_I 0.90000 + param set MC_ROLLRATE_D 0.00130 + + param set MC_PITCH_P 7.00000 + param set MC_PITCHRATE_P 0.15000 + param set MC_PITCHRATE_I 1.10000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.15 + param set MC_YAWRATE_D 0.0 + param set 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.00000 + param set MC_PITCHRATE_MAX 1600.00000 + param set MC_ROLLRATE_MAX 1600.00000 + param set MC_YAWRATE_MAX 700.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + param set MPC_MAN_TILT_MAX 35.0000 + param set MPC_TILTMAX_AIR 20.0000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_RATE 0 + + param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 2 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + param set MPC_XY_P 1.7000 + param set MPC_XY_VEL_P 0.1300 + param set MPC_XY_VEL_I 0.0600 + param set MPC_XY_VEL_D 0.0100 + param set MPC_TKO_RAMP_T 1.0000 + param set MPC_TKO_SPEED 1.1000 + param set MPC_VEL_MANUAL 3.0000 + + param set BAT_SOURCE 0 + param set CBRK_IO_SAFETY 22027 + param set COM_ARM_EKF_AB 0.00500 + param set COM_DISARM_LAND 3 + + # Filter settings + param set IMU_GYRO_CUTOFF 90.00000 + param set MC_DTERM_CUTOFF 70.00000 + + # 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 SDLOG_PROFILE 19 + + # 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 +fi diff --git a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 index dd8d8a89f9fa..2f84a2695340 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4080_zmr250 @@ -6,6 +6,7 @@ # @class Copter # # @board px4_fmu-v2 exclude +# @board intel_aerofc-v1 exclude # # @maintainer Anton Matosov # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind index 3ec3080d5632..d5d4599a1b47 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind +++ b/ROMFS/px4fmu_common/init.d/airframes/4090_nanomind @@ -7,10 +7,10 @@ # # @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 aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @maintainer Henry Zhang # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4250_teal b/ROMFS/px4fmu_common/init.d/airframes/4250_teal index 8629eeea57c9..f39268089a63 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4250_teal +++ b/ROMFS/px4fmu_common/init.d/airframes/4250_teal @@ -47,7 +47,7 @@ then param set CAL_MAG0_ROT 0 param set CAL_MAG_SIDES 63 param set SENS_BOARD_ROT 0 - param set COM_ARM_MAG 1.5 + param set COM_ARM_MAG_ANG -1 param set COM_ARM_EKF_AB 0.0032 # circuit breakers diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 24f911c5849b..3593ed9037f1 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -7,7 +7,8 @@ # @board px4_fmu-v4 exclude # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude -# @board aerofc-v1 exclude +# @board px4_fmu-v5x exclude +# @board intel_aerofc-v1 exclude # # @type Quadrotor x # @class Copter 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 7992a906eec0..3e6e91404d7d 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 @@ -66,8 +66,21 @@ then # Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor param set CBRK_AIRSPD_CHK 162128 + + # Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians + param set GND_MAX_ANG 3.1415 + + param set RBCLW_BAUD 8 + param set RBCLW_COUNTS_REV 1200 + param set RBCLW_ADDRESS 128 + # param set SER_TEL4_BAUD 115200 + # 104 corresponds to Telem 4 + param set RBCLW_SER_CFG 104 fi +# 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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r new file mode 100644 index 000000000000..95bad2029c91 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -0,0 +1,177 @@ +#!/bin/sh +# +# @name UVify Draco-R +# +# @type Hexarotor x +# @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 +# +# @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 intel_aerofc-v1 exclude +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# +# @maintainer Hyon Lim +# + +set VEHICLE_TYPE mc +set MIXER hexa_x +set PWM_OUT 123456 + +if [ $AUTOCNF = yes ] +then + # Attitude & rate gains + param set MC_ROLL_P 6.50000 + param set MC_ROLLRATE_P 0.15000 + param set MC_ROLLRATE_I 0.05000 + param set MC_ROLLRATE_D 0.00130 + + param set MC_PITCH_P 6.50000 + param set MC_PITCHRATE_P 0.15000 + param set MC_PITCHRATE_I 0.05000 + param set MC_PITCHRATE_D 0.00160 + + param set MC_YAW_P 2.80000 + param set MC_YAWRATE_P 0.20000 + param set MC_YAWRATE_I 0.10000 + param set MC_YAWRATE_D 0.00000 + param set MC_YAW_FF 0.00000 + + #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.00000 + param set MPC_MANTHR_MAX 0.90000 + param set MPC_MANTHR_MIN 0.08000 + param set MPC_MAN_TILT_MAX 35.0000 + param set MPC_TILTMAX_AIR 45.0000 + param set MPC_POS_MODE 2 + param set MPC_AUTO_MODE 1 + param set MPC_ACC_HOR 8.0000 + + param set MC_PITCHRATE_MAX 800.00000 + param set MC_ROLLRATE_MAX 800.00000 + param set MC_YAWRATE_MAX 700.00000 + + # Disable RC filtering + param set RC_FLT_CUTOFF 0.00000 + + # Filter settings + param set MC_DTERM_CUTOFF 90.00000 + param set IMU_GYRO_CUTOFF 100.00000 + + # Thrust curve (avoids the need for TPA) + param set THR_MDL_FAC 0.25 + + # System + param set PWM_MAX 1950 + param set PWM_MIN 1100 + param set PWM_MAIN_DIS5 980 + param set PWM_MAIN_DIS6 980 + + param set SYS_FMU_TASK 1 + param set SENS_BOARD_ROT 2 + + param set COM_ARM_MAG 0.2000 + + # Sensors + param set SENS_EN_LL40LS 2 + param set SENS_FLOW_ROT 2 + + # Position control + param set MPC_Z_P 1.00000 + param set MPC_Z_VEL_P 0.20000 + param set MPC_Z_VEL_I 0.02000 + param set MPC_Z_VEL_D 0.00000 + + param set MPC_THR_MIN 0.06000 + param set MPC_THR_MAX 0.40000 + param set MPC_THR_HOVER 0.3000 + + param set MIS_TAKEOFF_ALT 1.1000 + + param set MPC_XY_P 0.9500 + param set MPC_XY_VEL_P 0.0900 + param set MPC_XY_VEL_I 0.0200 + param set MPC_XY_VEL_D 0.0100 + + param set MPC_TKO_RAMP_T 0.4000 + param set MPC_TKO_SPEED 1.5000 + param set MPC_VEL_MANUAL 10.0000 + + # EKF + # Set baro first + param set EKF2_HGT_MODE 1 + # Enable optical flow and GPS + param set EKF2_AID_MASK 1 + param set EKF2_RNG_AID 0 + param set EKF2_MAG_TYPE 1 + param set EKF2_OF_QMIN 80.0000 + + # + param set CBRK_IO_SAFETY 22027 + param set SYS_COMPANION 921600 + param set COM_DISARM_LAND 3 + + #PWM + # ONESHOT + param set PWM_RATE 0 + + # gimbal + #param set MNT_DO_STAB 1 + #param set MNT_MAN_PITCH 1 + #param set MNT_MAN_ROLL 2 + #param set MNT_MODE_IN 1 + param set BAT_SOURCE 0 + param set BAT_N_CELLS 4 + param set BAT_V_DIV 10.133 + + # TELEM1 ttyS1 + param set MAV_0_CONFIG 101 + param set MAV_0_MODE 1 # onboard + param set MAV_0_FORWARD 1 + param set SER_TEL1_BAUD 57600 + + # TELEM2 ttyS2 + param set MAV_1_CONFIG 102 + param set MAV_1_MODE 2 + param set MAV_1_RATE 800000 + param set MAV_1_FORWARD 1 + param set SER_TEL2_BAUD 921600 +fi + +#set PWM_OUT 12345678 + +#set MIXER_AUX mount_2axes +#set PWM_AUX_OUT 78 +#set PWM_AUX_RATE 50 +#set OUTPUT_AUX_DEV /dev/pwm_output0 +#set OUTPUT_AUX_TO_MAIN yes +#set MIXER_APPEND yes + +#if mixer append /dev/pwm_output0 /etc/mixers/mount_2axes.aux.mix +#then +# echo "INFO [6002] Mixer append success" +#else +# echo "ERROR [6002] Mixer append failed" +#fi + +#if pwm rate -c 78 -r 50 -d /dev/pwm_output0 +#then +# echo "INFO [6002] PWM RATE CHANGE SUCCESS" +#else +# echo "INFO [6002] PWM RATE CHANGE FAILED" +#fi + diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f09ee37f6e67..e6cfe75e032a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -81,6 +81,8 @@ px4_add_romfs_files( 4053_holybro_kopis2 4060_dji_matrice_100 4070_aerofc + 4071_ifo + 4072_draco 4080_zmr250 4090_nanomind 4100_tiltquadrotor @@ -92,6 +94,7 @@ px4_add_romfs_files( # [6000, 6999] Hexarotor x" 6001_hexa_x + 6002_draco_r # [7000, 7999] Hexarotor +" 7001_hexa_+ @@ -129,6 +132,7 @@ px4_add_romfs_files( 13010_claire 13012_convergence 13013_deltaquad + 13200_generic_vtol_tailsitter # [14000, 14999] Tri Y 14001_tri_y_yaw+ diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index bcbf1fb0a523..c565aebf460c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -15,9 +15,8 @@ ekf2 start # fw_att_control start fw_pos_control_l1 start - +airspeed_selector start # # Start Land Detector. # land_detector start fixedwing - diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 520f9eb37e5a..54a717925620 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -30,6 +30,7 @@ then param set EKF2_REQ_SACC 1 param set EKF2_REQ_VDRIFT 1.0 + param set RTL_TYPE 1 param set RTL_RETURN_ALT 100 param set RTL_DESCEND_ALT 100 param set RTL_LAND_DELAY -1 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index d7971adebbe7..e53c40541fc0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -26,4 +26,3 @@ fi set MIXER_AUX pass set PWM_AUX_OUT 1234 -set PWM_AUX_RATE 50 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_defaults index 9f93fd61aa3f..5c8e952bc9df 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_defaults @@ -40,4 +40,3 @@ set PWM_RATE 50 # set MIXER_AUX pass set PWM_AUX_OUT 1234 -set PWM_AUX_RATE 50 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 208175a3d45a..7f0f5a53fa61 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -15,11 +15,6 @@ then fmu i2c 2 100000 fi - # External SPI - ms5611 -S start - - # Internal SPI (auto detect ms5611 or ms5607) - ms5611 -T 0 -s start adc start fi @@ -108,6 +103,12 @@ then teraranger start -a fi +# Possible pmw3901 optical flow sensor +if param greater SENS_EN_PMW3901 0 +then + pmw3901 start +fi + ############################################################################### # End Optional drivers # ############################################################################### diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 10ad0cdb7c9f..0861bdca563a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -21,10 +21,10 @@ mc_att_control start mc_pos_control start fw_att_control start fw_pos_control_l1 start +airspeed_selector start # # Start Land Detector # Multicopter for now until we have something for VTOL # land_detector start vtol - diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index f8f957568243..96b1b2a6d655 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -26,6 +26,7 @@ then param set PWM_RATE 400 param set RTL_LAND_DELAY 0 + param set RTL_TYPE 1 param set WV_EN 1 fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d0f00febbc3b..ee6ad4f93db0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -203,9 +203,12 @@ else rgbled_ncp5623c start rgbled_pwm start - if blinkm start + if param greater LIGHT_EN_BLINKM 0 then - blinkm systemstate + if blinkm start + then + blinkm systemstate + fi fi # @@ -215,11 +218,6 @@ else # tone_alarm start - if param compare SYS_FMU_TASK 1 - then - set FMU_ARGS "-t" - fi - # # Set parameters and env variables for selected AUTOSTART. # @@ -321,7 +319,7 @@ else then set OUTPUT_MODE hil sensors start -h - commander start --hil + commander start -h # disable GPS param set GPS_1_CONFIG 0 @@ -435,14 +433,6 @@ else # navigator start - # - # Start the standalone wind estimator. - # - if param compare WEST_EN 1 - then - wind_estimator start - fi - # # Start a thermal calibration if required. # @@ -457,7 +447,10 @@ else fi # Check for flow sensor, launched as a background task to scan - px4flow start & + if param compare SENS_EN_PX4FLOW 1 + then + px4flow start & + fi # Blacksheep telemetry if param greater TEL_BST_EN 0 diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.main.mix new file mode 100644 index 000000000000..b2215a8a5e78 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo.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- 10000 10000 10000 0 + +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. + +M: 2 +O: 7500 7500 0 -10000 10000 +S: 1 0 -10000 -10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 7500 7500 0 -10000 10000 +S: 1 0 -10000 -10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 040e2cff92de..2e6e7ac86c49 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -4,22 +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:2019-03-08" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-07-29" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then # posix_rpi_cross, posix_bebop_default - PX4_DOCKER_REPO="px4io/px4-dev-raspi:2019-03-08" + PX4_DOCKER_REPO="px4io/px4-dev-raspi:2019-07-29" elif [[ $@ =~ .*eagle.* ]] || [[ $@ =~ .*excelsior.* ]]; then # eagle, excelsior PX4_DOCKER_REPO="lorenzmeier/px4-dev-snapdragon:2018-09-12" elif [[ $@ =~ .*ocpoc.* ]]; then # aerotennaocpoc_ubuntu - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2019-03-08" - elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2019-07-29" + elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then # clang tools - PX4_DOCKER_REPO="px4io/px4-dev-clang:2019-03-08" + PX4_DOCKER_REPO="px4io/px4-dev-clang:2019-07-29" elif [[ $@ =~ .*tests* ]]; then # run all tests with simulation - PX4_DOCKER_REPO="px4io/px4-dev-simulation:2019-03-08" + PX4_DOCKER_REPO="px4io/px4-dev-simulation:2019-07-29" fi else echo "PX4_DOCKER_REPO is set to '$PX4_DOCKER_REPO'"; @@ -27,7 +27,7 @@ fi # otherwise default to nuttx if [ -z ${PX4_DOCKER_REPO+x} ]; then - PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-03-08" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx:2019-07-29" fi # docker hygiene diff --git a/Tools/jMAVSim b/Tools/jMAVSim index def7501bc053..3bd51e67e022 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit def7501bc0536b8d1050f65d09c7dfbebcc0ce61 +Subproject commit 3bd51e67e022ce59644d33ebdf6570c2ea88ddb6 diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh index a974edee272f..b04912afdd60 100755 --- a/Tools/jmavsim_run.sh +++ b/Tools/jmavsim_run.sh @@ -49,6 +49,10 @@ else device="-serial $device $baudrate" fi +if [ "$HEADLESS" = "1" ]; then + extra_args="$extr_args -no-gui" +fi + # jMAVSim crashes with Java 9 on macOS, therefore we need to use Java 8 if [ "$(uname)" == "Darwin" ]; then bold=$(tput bold) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index db4d37ad6112..3484576150c7 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -57,7 +57,8 @@ def mkdesc(): proto['version'] = "" proto['summary'] = "" proto['description'] = "" - proto['git_identity'] = "" + proto['git_identity'] = "" # git tag + proto['git_hash'] = "" # git commit hash proto['build_time'] = 0 proto['image'] = bytes() proto['image_size'] = 0 @@ -98,10 +99,14 @@ def mkdesc(): if args.description != None: desc['description'] = str(args.description) if args.git_identity != None: - cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--tags"]) + cmd = "git --git-dir '{:}/.git' describe --always --tags".format(args.git_identity) p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() + cmd = "git --git-dir '{:}/.git' rev-parse --verify HEAD".format(args.git_identity) + p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout + desc['git_hash'] = str(p.read().strip()) + p.close() if args.parameter_xml != None: f = open(args.parameter_xml, "rb") bytes = f.read() diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 2cdfd6a2be25..145bf773c38f 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -1,5 +1,17 @@ #! /usr/bin/env bash +## Bash script to setup PX4 development environment on Ubuntu LTS (18.04, 16.04). +## Can also be used in docker. +## +## Installs: +## - Common dependencies and tools for nuttx, jMAVSim, Gazebo +## - NuttX toolchain (omit with arg: --no-nuttx) +## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools) +## +## Not Installs: +## - FastRTPS and FastCDR + + INSTALL_NUTTX="true" INSTALL_SIM="true" @@ -33,6 +45,14 @@ fi # script directory DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) +# check requirements.txt exists (script not run in source tree) +REQUIREMENTS_FILE="requirements.txt" +if [[ ! -f "${DIR}/${REQUIREMENTS_FILE}" ]]; then + echo "FAILED: ${REQUIREMENTS_FILE} needed in same directory as ubuntu.sh (${DIR})." + return 1 +fi + + # check ubuntu version # instructions for 16.04, 18.04 # otherwise warn and point to docker? @@ -47,6 +67,8 @@ elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then echo "Ubuntu 18.04" fi + + export DEBIAN_FRONTEND=noninteractive echo @@ -73,6 +95,8 @@ sudo apt-get -yy --quiet --no-install-recommends install \ python3-pip \ python3-pygments \ python3-setuptools \ + python-pip \ + python-dev \ rsync \ shellcheck \ unzip \ @@ -82,12 +106,26 @@ sudo apt-get -yy --quiet --no-install-recommends install \ ; +if [[ "${UBUNTU_RELEASE}" == "16.04" ]]; then + echo "Installing Ubuntu 16.04 PX4-compatible cmake" + 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" sudo python3 -m pip install --upgrade pip setuptools wheel sudo python3 -m pip install -r ${DIR}/requirements.txt + +# Python2 dependencies +echo +echo "Installing PX4 Python2 dependencies" +sudo python2 -m pip install --upgrade pip setuptools wheel +sudo python2 -m pip install -r ${DIR}/requirements.txt + + # NuttX toolchain (arm-none-eabi-gcc) if [[ $INSTALL_NUTTX == "true" ]]; then @@ -148,14 +186,33 @@ if [[ $INSTALL_SIM == "true" ]]; then # java (jmavsim or fastrtps) sudo apt-get -yy --quiet --no-install-recommends install \ ant \ - default-jre-headless \ - default-jdk-headless \ + openjdk-8-jre \ + openjdk-8-jdk \ ; # Gazebo 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 - sudo apt-get update -yy --quiet - sudo apt-get -yy --quiet --no-install-recommends install gazebo9 + sudo apt-get -yy --quiet --no-install-recommends install \ + gazebo9 \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-ugly \ + libeigen3-dev \ + libgazebo9-dev \ + libgstreamer-plugins-base1.0-dev \ + libimage-exiftool-perl \ + libopencv-dev \ + libxml2-utils \ + pkg-config \ + protobuf-compiler \ + ; + +fi +if [[ $INSTALL_NUTTX == "true" ]]; then + echo + echo "Reboot computer before attempting to build NUTTX targets" fi diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 7a002bdd3049..651cfe1cd645 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -220,7 +220,7 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): # (the expectation is that the previous matching ORB_ID() will be passed # to this, so that we can ignore it) special_cases_sub = [ - ('sensors', r'voted_sensors_update\.cpp$', r'\binit_sensor_class\b\(([^,)]+)', r'^meta$'), + ('sensors', r'voted_sensors_update\.cpp$', r'\binitSensorClass\b\(([^,)]+)', r'^meta$'), ('mavlink', r'.*', r'\badd_orb_subscription\b\(([^,)]+)', r'^_topic$'), ('listener', r'.*', None, r'^(id)$'), ('logger', r'.*', None, r'^(topic|sub\.metadata|_polling_topic_meta)$'), @@ -229,7 +229,6 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): ('tap_esc', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('pwm_out_sim', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('snapdragon_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'), - ('fmu', r'.*', r'\b_control_topics\[[0-9]\]=([^,)]+)', r'^_control_topics\[i\]$'), ('linux_pwm_out', r'.*', r'\b_controls_topics\[[0-9]\]=([^,)]+)', r'^_controls_topics\[i\]$'), ] special_cases_sub = [(a, re.compile(b), re.compile(c) if c is not None else None, re.compile(d)) @@ -243,7 +242,7 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]): special_cases_pub = [ ('replay', r'replay_main\.cpp$', None, r'^sub\.orb_meta$'), ('fw_pos_control_l1', r'FixedwingPositionControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - + ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), diff --git a/Tools/upload.sh b/Tools/upload.sh index d12a3c7e8ba1..383f1a9b9e5f 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*," +SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify_FMU_BL*," fi if [[ $SYSTYPE = *"CYGWIN"* ]]; then diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 32d3e693afc1..da0ce7099cd7 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -53,7 +53,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib @@ -77,7 +77,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 72ca2384e2c3..a9efad64b755 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -15,6 +15,7 @@ px4_add_board( TEL2:/dev/ttyS2 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_trigger @@ -29,7 +30,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled lights/rgbled magnetometer # all available magnetometer drivers mkblctrl @@ -40,9 +40,6 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -73,7 +70,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -108,7 +105,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index cc60077ec1d1..c881b43db12d 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -14,3 +14,6 @@ mpu6000 -s -R 8 start mpu9250 -s -R 8 start lsm303d -R 10 start l3gd20 -R 14 start + +# Internal SPI +ms5611 -s start diff --git a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig index 8199ad51fe29..ef31f68f3102 100644 --- a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig +++ b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig @@ -20,7 +20,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -86,7 +86,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -98,7 +98,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -131,11 +130,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/airmind/mindpx-v2/src/timer_config.c b/boards/airmind/mindpx-v2/src/timer_config.c index c93957177e84..6f9b6fee53ac 100644 --- a/boards/airmind/mindpx-v2/src/timer_config.c +++ b/boards/airmind/mindpx-v2/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/atlflight/eagle/CMakeLists.txt b/boards/atlflight/eagle/CMakeLists.txt index d508e4c6d885..eaa46f8bafef 100644 --- a/boards/atlflight/eagle/CMakeLists.txt +++ b/boards/atlflight/eagle/CMakeLists.txt @@ -62,4 +62,12 @@ else() USES_TERMINAL ) + add_custom_target(sanity + COMMAND ./px4_snapflight_sanitytest.sh -i -t + DEPENDS px4 + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts + COMMENT "uploading px4" + USES_TERMINAL + ) + endif() diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 2ba781dfbe7c..d8d59a0bb4cb 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -88,7 +88,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index e03af1fab918..1a4b8badbb09 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -77,7 +77,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS param diff --git a/Tools/px4_snapflight_sanitytest.sh b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh similarity index 83% rename from Tools/px4_snapflight_sanitytest.sh rename to boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh index d62ca20e7894..24705af69e85 100755 --- a/Tools/px4_snapflight_sanitytest.sh +++ b/boards/atlflight/eagle/scripts/px4_snapflight_sanitytest.sh @@ -37,12 +37,12 @@ readonly EXIT_ERROR=3 # List of expected strings from the apps proc declare -a appsproc_strings_present=( - "on udp port 14556 remote port 14550" + "on udp port 14556 remote port 14550" ) # List of unexpected strings from the apps proc declare -a appsproc_strings_absent=( - "ERROR" + "ERROR" "Getting Bulk data from fastRPC link" "Segmentation fault" ) @@ -65,8 +65,9 @@ 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`/.. +workspace=`pwd`/../../../.. verifypx4test() { @@ -78,7 +79,7 @@ verifypx4test() { 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 @@ -126,14 +127,14 @@ verifypx4test() { 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 + fi } installpx4() { @@ -150,32 +151,17 @@ installpx4() { # 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/qurt_eagle_default/src/firmware/qurt/libpx4.so /usr/share/data/adsp - adb push $workspace/build/qurt_eagle_default/src/firmware/qurt/libpx4muorb_skel.so /usr/share/data/adsp - adb push $workspace/build/posix_eagle_default/src/firmware/posix/px4 /home/linaro + adb push $workspace/build/atlflight_eagle_qurt-default/platforms/qurt/libpx4.so /usr/share/data/adsp + adb push $workspace/build/atlflight_eagle_qurt-default/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 - elif [ $mode == 1 ]; then - # copy legacy binaries - echo -e "Copying the PX4 binaries from the eagle_legacy build tree..." - adb push $workspace/build/qurt_eagle_legacy/src/firmware/qurt/libpx4.so /usr/share/data/adsp - adb push $workspace/build/qurt_eagle_legacy/src/firmware/qurt/libpx4muorb_skel.so /usr/share/data/adsp - adb push $workspace/build/posix_eagle_legacy/src/firmware/posix/px4 /home/linaro - adb push $workspace/posix-configs/eagle/200qx/px4.config /usr/share/data/adsp - adb push $workspace/posix-configs/eagle/200qx/mainapp.config /home/linaro - else - echo -e "Copying the PX4 binaries from the excelsior_legacy build tree..." - adb push $workspace/build/qurt_excelsior_legacy/src/firmware/qurt/libpx4.so /usr/lib/rfsa/adsp - adb push $workspace/build/qurt_excelsior_legacy/src/firmware/qurt/libpx4muorb_skel.so /usr/lib/rfsa/adsp - adb push $workspace/build/posix_excelsior_legacy/src/firmware/posix/px4 /home/root - adb push $workspace/posix-configs/excelsior/px4.config /usr/lib/rfsa/adsp - adb push $workspace/posix-configs/excelsior/mainapp.config /home/root fi echo -e "Installation complete." @@ -188,13 +174,13 @@ testpx4() { 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 & @@ -207,7 +193,7 @@ testpx4() { exit $EXIT_ERROR fi - + # Start PX4 echo -e "Starting PX4..." if [ $mode == 2 ]; then @@ -224,19 +210,19 @@ testpx4() { 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." } @@ -281,7 +267,7 @@ while getopts "m:l:ith" opt; usage exit 0 ;; - :) + :) echo "Option -$OPTARG requires an argument" >&2 exit 1;; ?) diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 3715f82dba3d..a5c02c7b489a 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -88,7 +88,7 @@ px4_add_board( simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update @@ -118,7 +118,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index 69b7605ebc33..32bf468e63a4 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -77,7 +77,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS param diff --git a/boards/auav/esc35-v1/default.cmake b/boards/auav/esc35-v1/default.cmake index a5ce12d19682..2c36fae924ad 100644 --- a/boards/auav/esc35-v1/default.cmake +++ b/boards/auav/esc35-v1/default.cmake @@ -45,7 +45,6 @@ px4_add_board( DRIVERS bootloaders - stm32 uavcanesc MODULES diff --git a/boards/auav/esc35-v1/nuttx-config/nsh/defconfig b/boards/auav/esc35-v1/nuttx-config/nsh/defconfig index c32d2b5bf5bf..ea8f46f06b53 100644 --- a/boards/auav/esc35-v1/nuttx-config/nsh/defconfig +++ b/boards/auav/esc35-v1/nuttx-config/nsh/defconfig @@ -44,7 +44,7 @@ CONFIG_MAX_WDOGPARMS=2 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_NAME_MAX=93 -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=12 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_BUILTIN_APPS=y diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 110a05327cc8..7c6d692b368c 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -16,6 +16,7 @@ px4_add_board( TEL2:/dev/ttyS2 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -29,13 +30,11 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled lights/rgbled #lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl - lights/pca8574 #optical_flow # all available optical flow drivers #optical_flow/pmw3901 optical_flow/px4flow @@ -46,9 +45,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -79,7 +75,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -114,7 +110,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/auav/x21/init/rc.board_defaults b/boards/auav/x21/init/rc.board_defaults index 617e498f09ad..a247fc961b21 100644 --- a/boards/auav/x21/init/rc.board_defaults +++ b/boards/auav/x21/init/rc.board_defaults @@ -6,5 +6,5 @@ if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/boards/auav/x21/init/rc.board_sensors b/boards/auav/x21/init/rc.board_sensors index 8cb7043a91e8..4788196bd949 100644 --- a/boards/auav/x21/init/rc.board_sensors +++ b/boards/auav/x21/init/rc.board_sensors @@ -18,3 +18,6 @@ mpu6000 -R 2 -T 20602 start # Internal SPI bus mpu9250 is rotated 90 deg yaw mpu9250 -R 2 start + +# Internal SPI +ms5611 -s start diff --git a/boards/auav/x21/nuttx-config/nsh/defconfig b/boards/auav/x21/nuttx-config/nsh/defconfig index 18d36fde0f4c..e1349f9b5aa6 100644 --- a/boards/auav/x21/nuttx-config/nsh/defconfig +++ b/boards/auav/x21/nuttx-config/nsh/defconfig @@ -20,7 +20,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -85,7 +85,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -97,7 +97,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -130,11 +129,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/auav/x21/src/timer_config.c b/boards/auav/x21/src/timer_config.c index 66f375b0a665..6350d7c5e0df 100644 --- a/boards/auav/x21/src/timer_config.c +++ b/boards/auav/x21/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 63ba7cf443c6..e6ff77247bb1 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -31,8 +32,6 @@ px4_add_board( #imu # all available imu drivers irlock lights/blinkm - #lights/oreoled - #lights/pca8574 #lights/rgbled #lights/rgbled_ncp5623c #lights/rgbled_pwm @@ -47,9 +46,6 @@ px4_add_board( px4fmu rc_input #roboclaw - stm32 - stm32/adc - #stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -80,7 +76,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update @@ -114,7 +110,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 275fc7673573..c4e4d64f70c3 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -12,10 +12,9 @@ lsm303agr -R 4 start ms4525_airspeed -T 4515 -b 3 start -# try to autostart pmw3901 on SPI2 (external) -if ! pmw3901 start +if ! param greater SENS_EN_PMW3901 0 then - # if pmw3901 failed to start, try starting the adis16497 (SPI2) + # try to start adis16497 only if pmw3901 isn't enabled, or parameter doesn't exists adis16497 start fi @@ -24,4 +23,3 @@ ist8310 -C -b 1 start ist8310 -C -b 2 start hmc5883 -C -T -X start qmc5883 -X start - diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index be76a44adcc6..1ddcd0872eb2 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -44,7 +44,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32F777NI=y CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y @@ -122,7 +122,7 @@ CONFIG_NET_TCPBACKLOG=y CONFIG_NET_TCP_WRITE_BUFFERS=y CONFIG_NET_UDP=y CONFIG_NET_UDP_CHECKSUMS=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -130,7 +130,6 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_LOGIN_PASSWORD="px4" CONFIG_NSH_LOGIN_USERNAME="px4" @@ -157,11 +156,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index c3626213fcb8..f3b7c63d9605 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -66,7 +66,7 @@ #define GPIO_SPI1_RESET_ADIS16477 /* PB15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) /* SPI 2 CS */ -#define GPIO_SPI2_CS1_ADIS16497 /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN10) +#define GPIO_SPI2_CS1_ADIS16497 /* PI0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN0) /* SPI 4 CS */ #define GPIO_SPI4_CS1_LPS22HB /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) diff --git a/boards/av/x-v1/src/timer_config.c b/boards/av/x-v1/src/timer_config.c index e13e4924b70e..110e51274789 100644 --- a/boards/av/x-v1/src/timer_config.c +++ b/boards/av/x-v1/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" @@ -74,7 +74,7 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { { .base = STM32_TIM8_BASE, .clock_register = STM32_RCC_APB2ENR, - .clock_bit = RCC_APB2ENR_TIM10EN, + .clock_bit = RCC_APB2ENR_TIM8EN, .clock_freq = STM32_APB2_TIM8_CLKIN, .first_channel_index = 5, .last_channel_index = 5, diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 33f1dcafa0e6..14ac2630d878 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -51,7 +51,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib @@ -74,7 +74,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 3ddb553c445a..614944473573 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -49,7 +49,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS esc_calib @@ -72,7 +72,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 97a0ab9c58a7..b7bc44a4117f 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -14,7 +14,6 @@ px4_add_board( imu/mpu9250 optical_flow/pmw3901 px4fmu - stm32 MODULES attitude_estimator_q @@ -39,7 +38,7 @@ px4_add_board( temperature_compensation sih #vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/bitcraze/crazyflie/init/rc.board_sensors b/boards/bitcraze/crazyflie/init/rc.board_sensors index 929fb5268c39..58585770855c 100644 --- a/boards/bitcraze/crazyflie/init/rc.board_sensors +++ b/boards/bitcraze/crazyflie/init/rc.board_sensors @@ -12,4 +12,3 @@ lps25h start # Optical flow deck vl53lxx start -pmw3901 start diff --git a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig index 230182cad26d..b31d1424d718 100644 --- a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig @@ -20,7 +20,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F405RG=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -78,7 +78,7 @@ CONFIG_MTD=y CONFIG_MTD_AT24XX=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -91,7 +91,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_FATDEVNO=0 CONFIG_NSH_LINELEN=128 @@ -124,11 +123,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1600 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1600 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SEM_NNESTPRIO=8 diff --git a/boards/bitcraze/crazyflie/src/timer_config.c b/boards/bitcraze/crazyflie/src/timer_config.c index feedf26c7bbb..510ca6f5c687 100644 --- a/boards/bitcraze/crazyflie/src/timer_config.c +++ b/boards/bitcraze/crazyflie/src/timer_config.c @@ -48,7 +48,7 @@ #include "board_config.h" #include -#include +#include /* IO Timers normally free-run at 1MHz diff --git a/boards/bitcraze/crazyflie/syslink/CMakeLists.txt b/boards/bitcraze/crazyflie/syslink/CMakeLists.txt index 311abb73fe4e..f740aab0fd77 100644 --- a/boards/bitcraze/crazyflie/syslink/CMakeLists.txt +++ b/boards/bitcraze/crazyflie/syslink/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( MAIN syslink COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable - STACK_MAIN 1300 SRCS syslink_main.cpp syslink_bridge.cpp diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 287a5d155f44..2375b3df7a72 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -56,7 +56,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn @@ -82,7 +82,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 39ea8e0336ba..06da561ba47b 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -54,7 +54,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn @@ -80,7 +80,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp b/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp index 5241a4009707..009403883824 100644 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp +++ b/boards/emlid/navio2/navio_rgbled/navio_rgbled.cpp @@ -55,13 +55,6 @@ RGBLED::RGBLED(const char *name) { }; -RGBLED::~RGBLED() -{ - if (_led_controller.led_control_subscription() >= 0) { - orb_unsubscribe(_led_controller.led_control_subscription()); - } -}; - int RGBLED::start() { int res = DevObj::init(); @@ -133,15 +126,14 @@ int RGBLED::start() return res; } -int RGBLED::stop() +int +RGBLED::stop() { - int res; - _gpioR.unexportPin(); _gpioG.unexportPin(); _gpioB.unexportPin(); - res = DevObj::stop(); + int res = DevObj::stop(); if (res < 0) { DF_LOG_ERR("could not stop DevObj"); @@ -152,13 +144,9 @@ int RGBLED::stop() return 0; } -void RGBLED::_measure() +void +RGBLED::_measure() { - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } - LedControlData led_control_data; if (_led_controller.update(led_control_data) == 1) { diff --git a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h b/boards/emlid/navio2/navio_rgbled/navio_rgbled.h index aa25935dcd17..55fc3f8fa86b 100644 --- a/boards/emlid/navio2/navio_rgbled/navio_rgbled.h +++ b/boards/emlid/navio2/navio_rgbled/navio_rgbled.h @@ -41,7 +41,7 @@ class RGBLED : public DriverFramework::DevObj { public: RGBLED(const char *name); - virtual ~RGBLED(); + virtual ~RGBLED() = default; int start(); int stop(); diff --git a/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt b/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt index 2785a22b1191..6bb2164df191 100644 --- a/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt +++ b/boards/emlid/navio2/navio_sysfs_rc_in/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__navio_sysfs_rc_in MAIN navio_sysfs_rc_in - STACK_MAIN 1200 COMPILE_FLAGS SRCS navio_sysfs_rc_in.cpp diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 393316ea0706..2548c0aa7164 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -17,6 +17,7 @@ px4_add_board( # /dev/ttyS5: UART7 (ESC telemetry) DRIVERS + adc barometer/bmp280 gps imu/mpu6000 @@ -25,9 +26,6 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - stm32/tone_alarm telemetry tone_alarm osd diff --git a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index cd30187d0ae4..023d19292884 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -87,7 +87,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y diff --git a/boards/holybro/kakutef7/src/timer_config.c b/boards/holybro/kakutef7/src/timer_config.c index cb5827af77ee..325d6a160890 100644 --- a/boards/holybro/kakutef7/src/timer_config.c +++ b/boards/holybro/kakutef7/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 28d0aae82ceb..acf8e39884fd 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -27,7 +27,6 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 tap_esc #telemetry # all available telemetry drivers #uavcan @@ -56,7 +55,7 @@ px4_add_board( sih vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/intel/aerofc-v1/init/rc.board_sensors b/boards/intel/aerofc-v1/init/rc.board_sensors index cece9a3aaf81..e2b090fb7ec7 100644 --- a/boards/intel/aerofc-v1/init/rc.board_sensors +++ b/boards/intel/aerofc-v1/init/rc.board_sensors @@ -14,3 +14,6 @@ ist8310 -C -b 1 -R 4 start aerofc_adc start ll40ls start i2c + +# Internal SPI (auto detect ms5611 or ms5607) +ms5611 -T 0 -s start diff --git a/boards/intel/aerofc-v1/nuttx-config/nsh/defconfig b/boards/intel/aerofc-v1/nuttx-config/nsh/defconfig index b391f9c084a7..b18eb8a4e7fe 100644 --- a/boards/intel/aerofc-v1/nuttx-config/nsh/defconfig +++ b/boards/intel/aerofc-v1/nuttx-config/nsh/defconfig @@ -17,7 +17,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F429V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -68,7 +68,7 @@ CONFIG_MM_REGIONS=2 CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -80,7 +80,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -111,11 +110,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SEM_NNESTPRIO=8 diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 5172bdc4f4ea..79a01af8c90b 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -30,7 +30,6 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 tap_esc #telemetry # all available telemetry drivers #uavcan @@ -60,7 +59,7 @@ px4_add_board( sih vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/intel/aerofc-v1/src/timer_config.c b/boards/intel/aerofc-v1/src/timer_config.c index b9385b2fbeef..887e5013e97f 100644 --- a/boards/intel/aerofc-v1/src/timer_config.c +++ b/boards/intel/aerofc-v1/src/timer_config.c @@ -47,7 +47,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake new file mode 100644 index 000000000000..0648dd8e869e --- /dev/null +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -0,0 +1,122 @@ + +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 + #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 + gps + #heater + #imu # all available imu drivers + #imu/bmi088 + imu/mpu6000 + imu/icm20948 + irlock + #lights/blinkm + lights/rgbled + magnetometer # all available magnetometer drivers + #md25 + mkblctrl + #optical_flow # all available optical flow drivers + pca9685 + #protocol_splitter + #pwm_input + pwm_out_sim + px4fmu + rc_input + roboclaw + safety_button + tap_esc + telemetry # all available telemetry drivers + test_ppm + tone_alarm + uavcan + + MODULES + attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + fw_att_control + fw_pos_control_l1 + rover_pos_control + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + navigator + sensors + sih + vmount + vtol_att_control + airspeed_selector + + SYSTEMCMDS + bl_update + config + dmesg + dumpfile + esc_calib + hardfault_log + i2cdetect + led_control + 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 + usb_connected + ver + + 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 + segway + uuv_example_app + + ) diff --git a/boards/mro/ctrl-zero-f7/firmware.prototype b/boards/mro/ctrl-zero-f7/firmware.prototype new file mode 100644 index 000000000000..c3f122993a34 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 141, + "magic": "mRo-ctrl-zero-f7", + "description": "Firmware for the mRo-ctrl-zero-f7 board", + "image": "", + "build_time": 0, + "summary": "mRo-ctrl-zero-f7", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2064384, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_defaults b/boards/mro/ctrl-zero-f7/init/rc.board_defaults new file mode 100644 index 000000000000..2930b72d65e3 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/init/rc.board_defaults @@ -0,0 +1,15 @@ +#!/bin/sh +# +# mRo Control Zero specific board defaults +#------------------------------------------------------------------------------ + + +if [ $AUTOCNF = yes ] +then + +fi + +set LOGGER_BUF 64 +set MIXER_AUX none + +safety_button start diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_sensors b/boards/mro/ctrl-zero-f7/init/rc.board_sensors new file mode 100644 index 000000000000..c240ae8413bd --- /dev/null +++ b/boards/mro/ctrl-zero-f7/init/rc.board_sensors @@ -0,0 +1,22 @@ +#!/bin/sh +# +# mRo Control Zero specific board sensors init +#------------------------------------------------------------------------------ + +# Internal ICM-20602 +mpu6000 -R 10 -s -T 20602 start + +# Internal ICM-20689 +#icm20689 -R 10 20689 start + +# Internal BMI088 +bmi088 start + +# Interal DPS310 (barometer) +dps310 start + +# Possible external compasses +#ist8310 -C -b 1 start +#ist8310 -C -b 2 start +#hmc5883 -C -T -X start +#qmc5883 -X start diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-f7/nuttx-config/include/board.h new file mode 100644 index 000000000000..f6a143f3bc96 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/nuttx-config/include/board.h @@ -0,0 +1,380 @@ +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The mRo Control Zero F7 board provides the following clock sources: + * + * 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 24 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 24000000ul +#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_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 + * + * SDMMC and RNG Clock = PLL_VCO / PLLQ + * Subject to + * The SDMMC and the random number generator need a frequency lower than or equal + * to 48 MHz to work correctly. + * + * 2 <= PLLQ <= 15 + */ + +/* SDMMCCLK (= USB OTG FS clock = RNGCLK) should be <= 48MHz + * + * PLL_VCO = (16,000,000 / 24) * 432 = 432 MHz + * SYSCLK = 432 MHz / 2 = 216 MHz + * SDMMC and RNG Clock = 432 MHz / 9 = 48 MHz + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(432) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(9) + +#define STM32_VCO_FREQUENCY ((STM32_HSE_FREQUENCY / 24) * 432) +#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 + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * if we set aside more DMA channels/streams. + * + * SDMMC DMA is on DMA2 + * + * SDMMC1 DMA + * DMAMAP_SDMMC1_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + * DMAMAP_SDMMC1_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDMMC1 DMAMAP_SDMMC1_1 + + +/* 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 + +/* Alternate function pin selections ************************************************/ + +#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_1 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_1 /* PA0 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ + +/* USART8: has no remap + * + * GPIO_UART8_RX PE0 + * GPIO_UART8_TX PE1 + */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + + +/* 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 sensors 1 + * SPI2 FRAM + baro + * SPI5 sensors 2 + * SPI6 Reserved + * + */ +#define GPIO_SPI1_SCK 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 GPIO_SPI2_SCK_2 /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#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_1 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +/* SDMMC1 + * + * VDD 3.3 + * GND + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + */ + +/************************************************************************************ + * Public Data + ************************************************************************************/ +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#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. + * + ************************************************************************************/ + +void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..cd7774f62b11 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/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_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_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_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP_STM32F777NI=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_MATH_H=y +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=0x008D +CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroF7" +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_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_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_EXCLUDE_BLOCKS=y +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_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_MAX_WDOGPARMS=2 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=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_NFILE_DESCRIPTORS=20 +CONFIG_NFILE_STREAMS=8 +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_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +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_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_PIPES=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PREALLOC_WDOGS=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_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=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=32 +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_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=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_SPI2=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM1=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM4=y +CONFIG_STM32F7_TIM9=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_TIME_EXTENDED=y +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_RXDMA=y +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_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=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-f7/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..d6188a20ef1d --- /dev/null +++ b/boards/mro/ctrl-zero-f7/nuttx-config/scripts/script.ld @@ -0,0 +1,183 @@ +/**************************************************************************** + * 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 +{ + itcm (rwx) : ORIGIN = 0x00218000, LENGTH = 1952K + flash (rx) : ORIGIN = 0x08018000, LENGTH = 1952K /* start on 4th sector (1st sector for bootloader, 2 for extra storage) */ + dtcm (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) + +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(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = 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/mro/ctrl-zero-f7/src/CMakeLists.txt b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt new file mode 100644 index 000000000000..9e3ed3b419cf --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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(drivers_board + init.c + led.c + sdio.c + spi.cpp + timer_config.c + usb.c +) + +target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h new file mode 100644 index 000000000000..2ec6241e0934 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -0,0 +1,397 @@ +/**************************************************************************** + * + * 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 + * + * mro_ctrl-zero-f7 internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* GPIOs ***********************************************************************************/ + +/* 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 + +#define PX4_SPI_BUS_1 1 +#define PX4_SPI_BUS_2 2 +#define PX4_SPI_BUS_5 5 + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +/* Define the Chip Selects, Data Ready and Control signals per SPI bus */ + +/* SPI 1 CS */ +#define GPIO_SPI1_CS1_ICM20602 /* PC2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI1_CS2_ICM20948 /* PE15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + +/* SPI 2 CS */ +#define GPIO_SPI2_CS1_FRAM /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI2_CS2_BARO /* PD7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) + +/* SPI 5 CS */ +#define GPIO_SPI5_CS1_BMI088_ACCEL /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN6) +#define GPIO_SPI5_CS2_BMI088_GYRO /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN10) + +/* SPI 5 BMI088 Data Ready interrupts */ +#define GPIO_DRDY_BMI088_INT1_ACCEL /* PF1 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTF|GPIO_PIN1) +#define GPIO_DRDY_BMI088_INT2_ACCEL /* PF2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN2) +#define GPIO_DRDY_BMI088_INT3_GYRO /* PF3 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTF|GPIO_PIN3) +#define GPIO_DRDY_BMI088_INT4_GYRO /* PF4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN4) + +/* v BEGIN Legacy SPI defines TODO: fix this with enumeration */ +#define PX4_SPI_BUS_RAMTRON PX4_SPI_BUS_2 +/* ^ END Legacy SPI defines TODO: fix this with enumeration */ + +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_1,0) +#define PX4_SPIDEV_ICM_20948 PX4_MK_SPI_SEL(PX4_SPI_BUS_1,1) +#define PX4_SPI_BUS_1_CS_GPIO {GPIO_SPI1_CS1_ICM20602, GPIO_SPI1_CS2_ICM20948} + +#define PX4_SPIDEV_MEMORY PX4_MK_SPI_SEL(PX4_SPI_BUS_2,0) +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_2,1) +#define PX4_SPI_BUS_2_CS_GPIO {GPIO_SPI2_CS1_FRAM, GPIO_SPI2_CS2_BARO} + +#define PX4_SPIDEV_BMI088_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_5,0) +#define PX4_SPIDEV_BMI088_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_5,1) +#define PX4_SPI_BUS_5_CS_GPIO {GPIO_SPI5_CS1_BMI088_ACCEL, GPIO_SPI5_CS2_BMI088_GYRO} + + +/* I2C busses */ + +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +#define BOARD_NUMBER_I2C_BUSES 1 +#define BOARD_I2C_BUS_CLOCK_INIT {100000} + +/* + * 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 \ + /* PA2 */ ADC1_GPIO(2), \ + /* PA3 */ ADC1_GPIO(3), \ + /* PA4 */ ADC1_GPIO(4), \ + /* PC1 */ ADC1_GPIO(11) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PA2 */ ADC1_CH(2) +#define ADC_BATTERY_CURRENT_CHANNEL /* PA3 */ ADC1_CH(3) +#define ADC_SCALED_V5_CHANNEL /* PA4 */ ADC1_CH(4) +#define ADC_RSSI_IN_CHANNEL /* PC1 */ ADC1_CH(11) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL)) + +/* Define Battery 1 Voltage Divider and A per V */ +#define BOARD_BATTERY_V_DIV (18.1f) /* measured with the provided PM board */ +#define BOARD_BATTERY_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) + +/* CAN Silence: Silent mode control \ ESC Mux select */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM + * + * 8 PWM outputs are configured. + * + * Pins: + * + * FMU_CH1 : PE14 : TIM1_CH4 + * FMU_CH2 : PE13 : TIM1_CH3 + * FMU_CH3 : PE11 : TIM1_CH2 + * FMU_CH4 : PE9 : TIM1_CH1 + * FMU_CH5 : PD13 : TIM4_CH2 + * FMU_CH6 : PD14 : TIM4_CH3 + * FMU_CH7 : PI5 : TIM8_CH1 + * FMU_CH8 : PI6 : TIM8_CH2 + * + */ +#define GPIO_TIM8_CH2OUT /* PI6 T8C1 FMU8 */ GPIO_TIM8_CH2OUT_2 +#define GPIO_TIM8_CH1OUT /* PI5 T8C1 FMU7 */ GPIO_TIM8_CH1OUT_2 +#define GPIO_TIM4_CH3OUT /* PD14 T4C3 FMU6 */ GPIO_TIM4_CH3OUT_2 +#define GPIO_TIM4_CH2OUT /* PD13 T4C2 FMU5 */ GPIO_TIM4_CH2OUT_2 +#define GPIO_TIM1_CH1OUT /* PE9 T1C1 FMU4 */ GPIO_TIM1_CH1OUT_2 +#define GPIO_TIM1_CH2OUT /* PE11 T1C2 FMU3 */ GPIO_TIM1_CH2OUT_2 +#define GPIO_TIM1_CH3OUT /* PE13 T1C3 FMU2 */ GPIO_TIM1_CH3OUT_2 +#define GPIO_TIM1_CH4OUT /* PE14 T1C4 FMU1 */ GPIO_TIM1_CH4OUT_2 + +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define GPIO_TIM8_CH2IN /* PI6 T8C2 FMU8 */ GPIO_TIM8_CH2IN_2 +#define GPIO_TIM8_CH1IN /* PI5 T8C1 FMU7 */ GPIO_TIM8_CH1IN_2 +#define GPIO_TIM4_CH3IN /* PD14 T4C3 FMU6 */ GPIO_TIM4_CH3IN_2 +#define GPIO_TIM4_CH2IN /* PD13 T4C2 FMU5 */ GPIO_TIM4_CH2IN_2 +#define GPIO_TIM1_CH1IN /* PE9 T1C1 FMU4 */ GPIO_TIM1_CH1IN_2 +#define GPIO_TIM1_CH2IN /* PE11 T1C2 FMU3 */ GPIO_TIM1_CH2IN_2 +#define GPIO_TIM1_CH3IN /* PE13 T1C3 FMU2 */ GPIO_TIM1_CH3IN_2 +#define GPIO_TIM1_CH4IN /* PE14 T1C4 FMU1 */ GPIO_TIM1_CH4IN_2 + +#define DIRECT_INPUT_TIMER_CHANNELS 8 + +/* User GPIOs: GPIO0-5 are the PWM servo outputs. */ +#define _MK_GPIO_INPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLUP)) + +#define GPIO_GPIO7_INPUT /* PI6 T8C2 FMU8 */ _MK_GPIO_INPUT(GPIO_TIM8_CH2IN) +#define GPIO_GPIO6_INPUT /* PI5 T8C1 FMU7 */ _MK_GPIO_INPUT(GPIO_TIM8_CH1IN) +#define GPIO_GPIO5_INPUT /* PD14 T4C3 FMU6 */ _MK_GPIO_INPUT(GPIO_TIM4_CH3IN) +#define GPIO_GPIO4_INPUT /* PD13 T4C2 FMU5 */ _MK_GPIO_INPUT(GPIO_TIM4_CH2IN) +#define GPIO_GPIO3_INPUT /* PE9 T1C1 FMU4 */ _MK_GPIO_INPUT(GPIO_TIM1_CH1IN) +#define GPIO_GPIO2_INPUT /* PE11 T1C2 FMU3 */ _MK_GPIO_INPUT(GPIO_TIM1_CH2IN) +#define GPIO_GPIO1_INPUT /* PE13 T1C3 FMU2 */ _MK_GPIO_INPUT(GPIO_TIM1_CH3IN) +#define GPIO_GPIO0_INPUT /* PE14 T1C4 FMU1 */ _MK_GPIO_INPUT(GPIO_TIM1_CH4IN) + +#define _MK_GPIO_OUTPUT(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR)) + +#define GPIO_GPIO7_OUTPUT /* PI6 T8C2 FMU8 */ _MK_GPIO_OUTPUT(GPIO_TIM8_CH2OUT) +#define GPIO_GPIO6_OUTPUT /* PI5 T8C1 FMU7 */ _MK_GPIO_OUTPUT(GPIO_TIM8_CH1OUT) +#define GPIO_GPIO5_OUTPUT /* PD14 T4C3 FMU6 */ _MK_GPIO_OUTPUT(GPIO_TIM4_CH3OUT) +#define GPIO_GPIO4_OUTPUT /* PD13 T4C2 FMU5 */ _MK_GPIO_OUTPUT(GPIO_TIM4_CH2OUT) +#define GPIO_GPIO3_OUTPUT /* PE9 T1C1 FMU4 */ _MK_GPIO_OUTPUT(GPIO_TIM1_CH1OUT) +#define GPIO_GPIO2_OUTPUT /* PE11 T1C2 FMU3 */ _MK_GPIO_OUTPUT(GPIO_TIM1_CH2OUT) +#define GPIO_GPIO1_OUTPUT /* PA10 T1C3 FMU2 */ _MK_GPIO_OUTPUT(GPIO_TIM1_CH3OUT) +#define GPIO_GPIO0_OUTPUT /* PE14 T1C4 FMU1 */ _MK_GPIO_OUTPUT(GPIO_TIM1_CH4OUT) + + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_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 BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) +#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_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, (on_true)) +#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) + +/* 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 + * + * 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 /* T3C2 */ 2 /* use capture/compare channel 1 */ +#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" +#define RC_SERIAL_SINGLEWIRE + +#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) + +/* + * FMUv5 has a separate RC_IN + * + * GPIO PPM_IN on PC7 T3CH2 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PG9 (NOT FMUv5 test HW ONLY) + * In version 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)) + +#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 +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_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 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +/* The list of GPIO that will be initialized */ +#define PX4_GPIO_PWM_INIT_LIST { \ + GPIO_GPIO7_INPUT, \ + GPIO_GPIO6_INPUT, \ + GPIO_GPIO5_INPUT, \ + GPIO_GPIO4_INPUT, \ + GPIO_GPIO3_INPUT, \ + GPIO_GPIO2_INPUT, \ + GPIO_GPIO1_INPUT, \ + GPIO_GPIO0_INPUT, \ + } + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SENSORS_EN, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_DRDY_BMI088_INT1_ACCEL, \ + GPIO_DRDY_BMI088_INT2_ACCEL, \ + GPIO_DRDY_BMI088_INT3_GYRO, \ + GPIO_DRDY_BMI088_INT4_GYRO, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +__BEGIN_DECLS + +#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); + +void board_spi_reset(int ms); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization for NSH. + * + * CONFIG_NSH_ARCHINIT=y : + * Called from the NSH library + * + * CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, && + * CONFIG_NSH_ARCHINIT=n : + * Called from board_initialize(). + * + ****************************************************************************/ + +#ifdef CONFIG_NSH_LIBRARY +int nsh_archinitialize(void); +#endif + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c new file mode 100644 index 000000000000..5d376671cee6 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * 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 init.c + * + * Board-specific early startup code. This file implements the + * nsh_archinitialize() 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 "up_internal.h" + +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void 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_3V3_SENSORS_EN(false); + + 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); + VDD_3V3_SENSORS_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) +{ + /* configure the GPIO pins to outputs and keep them low */ + + const uint32_t gpio[] = PX4_GPIO_PWM_INIT_LIST; + board_gpio_init(gpio, arraySize(gpio)); + + 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; + board_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; the meaning of the argument is a contract + * between the board-specific initialization 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_SENSORS_EN(true); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* set up the serial DMA polling */ + 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); + + + /* 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); + return ret; + } + +#endif /* CONFIG_MMCSD */ + + return OK; +} diff --git a/boards/mro/ctrl-zero-f7/src/led.c b/boards/mro/ctrl-zero-f7/src/led.c new file mode 100644 index 000000000000..e68ea7fd8dde --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/led.c @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 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 up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void 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++) { + 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(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-f7/src/sdio.c b/boards/mro/ctrl-zero-f7/src/sdio.c new file mode 100644 index 000000000000..bb56cc8a8be7 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/sdio.c @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * 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 = !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/mro/ctrl-zero-f7/src/spi.cpp b/boards/mro/ctrl-zero-f7/src/spi.cpp new file mode 100644 index 000000000000..895184c5887a --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/spi.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * 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 spi.cpp + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include "board_config.h" + +/* Define CS GPIO array */ +static constexpr uint32_t spi1selects_gpio[] = PX4_SPI_BUS_1_CS_GPIO; +static constexpr uint32_t spi2selects_gpio[] = PX4_SPI_BUS_2_CS_GPIO; +static constexpr uint32_t spi5selects_gpio[] = PX4_SPI_BUS_5_CS_GPIO; + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize() +{ +#ifdef CONFIG_STM32F7_SPI1 + + for (auto gpio : spi1selects_gpio) { + px4_arch_configgpio(gpio); + } + +#endif // CONFIG_STM32F7_SPI1 + + +#if defined(CONFIG_STM32F7_SPI2) + + for (auto gpio : spi2selects_gpio) { + px4_arch_configgpio(gpio); + } + +#endif // CONFIG_STM32F7_SPI2 + +#ifdef CONFIG_STM32F7_SPI5 + + for (auto gpio : spi5selects_gpio) { + px4_arch_configgpio(gpio); + } + +#endif // CONFIG_STM32F7_SPI5 +} + +/************************************************************************************ + * Name: stm32_spi1select and stm32_spi1status + * + * Description: + * Called by stm32 spi driver on bus 1. + * + ************************************************************************************/ +#ifdef CONFIG_STM32F7_SPI1 +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_1); + + // Making sure the other peripherals are not selected + for (auto cs : spi1selects_gpio) { + stm32_gpiowrite(cs, 1); + } + + // SPI select is active low, so write !selected to select the device + stm32_gpiowrite(spi1selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_STM32F7_SPI1 + +/************************************************************************************ + * Name: stm32_spi2select and stm32_spi2status + * + * Description: + * Called by stm32 spi driver on bus 2. + * + ************************************************************************************/ +#if defined(CONFIG_STM32F7_SPI2) +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + if (devid == SPIDEV_FLASH(0)) { + devid = PX4_SPIDEV_MEMORY; + } + + ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_2); + + // Making sure the other peripherals are not selected + for (auto cs : spi2selects_gpio) { + stm32_gpiowrite(cs, 1); + } + + // SPI select is active low, so write !selected to select the device + stm32_gpiowrite(spi2selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_STM32F7_SPI2 && GPIO_SPI2_CS_FRAM + +/************************************************************************************ + * Name: stm32_spi5select and stm32_spi5status + * + * Description: + * Called by stm32 spi driver on bus 5. + * + ************************************************************************************/ +#ifdef CONFIG_STM32F7_SPI5 +__EXPORT void stm32_spi5select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + ASSERT(PX4_SPI_BUS_ID(devid) == PX4_SPI_BUS_5); + + // Making sure the other peripherals are not selected + for (auto cs : spi5selects_gpio) { + stm32_gpiowrite(cs, 1); + } + + // SPI select is active low, so write !selected to select the device + stm32_gpiowrite(spi5selects_gpio[PX4_SPI_DEV_ID(devid)], !selected); +} + +__EXPORT uint8_t stm32_spi5status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif // CONFIG_STM32F7_SPI5 + +/************************************************************************************ + * Name: board_spi_reset + * + * Description: + * + * + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms) +{ + // disable SPI bus + + // SPI1 + for (auto cs : spi1selects_gpio) { + stm32_configgpio(_PIN_OFF(cs)); + } + + stm32_configgpio(_PIN_OFF(GPIO_SPI1_SCK)); + stm32_configgpio(_PIN_OFF(GPIO_SPI1_MISO)); + stm32_configgpio(_PIN_OFF(GPIO_SPI1_MOSI)); + + // SPI5 + for (auto cs : spi5selects_gpio) { + stm32_configgpio(_PIN_OFF(cs)); + } + + stm32_configgpio(_PIN_OFF(GPIO_SPI5_SCK)); + stm32_configgpio(_PIN_OFF(GPIO_SPI5_MISO)); + stm32_configgpio(_PIN_OFF(GPIO_SPI5_MOSI)); + stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT1_ACCEL)); + stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT2_ACCEL)); + stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT3_GYRO)); + stm32_configgpio(_PIN_OFF(GPIO_DRDY_BMI088_INT4_GYRO)); + + /* set the sensor rail off */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* 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 */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ + + // SPI1 + for (auto cs : spi1selects_gpio) { + stm32_configgpio(cs); + } + + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + + // SPI5 + for (auto cs : spi5selects_gpio) { + stm32_configgpio(cs); + } + + stm32_configgpio(GPIO_SPI5_SCK); + stm32_configgpio(GPIO_SPI5_MISO); + stm32_configgpio(GPIO_SPI5_MOSI); + stm32_configgpio(GPIO_DRDY_BMI088_INT1_ACCEL); + stm32_configgpio(GPIO_DRDY_BMI088_INT2_ACCEL); + stm32_configgpio(GPIO_DRDY_BMI088_INT3_GYRO); + stm32_configgpio(GPIO_DRDY_BMI088_INT4_GYRO); +} diff --git a/boards/mro/ctrl-zero-f7/src/timer_config.c b/boards/mro/ctrl-zero-f7/src/timer_config.c new file mode 100644 index 000000000000..bc6a90aed925 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/timer_config.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM1CC, + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .first_channel_index = 4, + .last_channel_index = 5, + .handler = io_timer_handler1, + .vectorno = STM32_IRQ_TIM4, + }, + { + .base = STM32_TIM8_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM8EN, + .clock_freq = STM32_APB2_TIM8_CLKIN, + .first_channel_index = 6, + .last_channel_index = 7, + .handler = io_timer_handler2, + .vectorno = STM32_IRQ_TIM8UP, + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM1_CH4OUT, + .gpio_in = GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + }, + { + .gpio_out = GPIO_TIM1_CH3OUT, + .gpio_in = GPIO_TIM1_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM1_CH2OUT, + .gpio_in = GPIO_TIM1_CH2IN, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM1_CH1OUT, + .gpio_in = GPIO_TIM1_CH1IN, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM4_CH2OUT, + .gpio_in = GPIO_TIM4_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM4_CH3OUT, + .gpio_in = GPIO_TIM4_CH3IN, + .timer_index = 1, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM8_CH1OUT, + .gpio_in = GPIO_TIM8_CH1IN, + .timer_index = 2, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM8_CH2OUT, + .gpio_in = GPIO_TIM8_CH2IN, + .timer_index = 2, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + } +}; diff --git a/boards/mro/ctrl-zero-f7/src/usb.c b/boards/mro/ctrl-zero-f7/src/usb.c new file mode 100644 index 000000000000..8f78ff420124 --- /dev/null +++ b/boards/mro/ctrl-zero-f7/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 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 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/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index d2e3f46b9fd1..a0eceedc9ec2 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -15,6 +15,7 @@ px4_add_board( TEL2:/dev/ttyS1 DRIVERS + adc barometer # all available barometer drivers barometer/mpl3115a2 batt_smbus @@ -30,11 +31,7 @@ px4_add_board( imu/mpu6000 imu/mpu9250 irlock - kinetis - kinetis/adc - kinetis/tone_alarm lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm @@ -46,6 +43,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input + safety_button tap_esc telemetry # all available telemetry drivers #test_ppm # NOT Portable YET @@ -75,10 +73,11 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update + i2cdetect config dumpfile esc_calib @@ -109,7 +108,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index 52739b0d634c..2d81e88031cf 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -9,3 +9,5 @@ then fi +safety_button start + diff --git a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig index e1bf4c51ef78..14d412c5334f 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig @@ -6,7 +6,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_KINETIS=y CONFIG_ARCH_CHIP_MK66FN2M0VMD18=y -CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -107,7 +107,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -125,7 +125,6 @@ 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 CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -163,10 +162,10 @@ CONFIG_RTC=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SEM_NNESTPRIO=8 diff --git a/boards/nxp/fmuk66-v3/src/timer_config.c b/boards/nxp/fmuk66-v3/src/timer_config.c index 3d61c2041e74..67eac5cd51e7 100644 --- a/boards/nxp/fmuk66-v3/src/timer_config.c +++ b/boards/nxp/fmuk66-v3/src/timer_config.c @@ -47,7 +47,7 @@ #include "chip/kinetis_ftm.h" #include -#include +#include #include "board_config.h" diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 7c1e7fbd49c2..e8237288877f 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -12,6 +12,7 @@ px4_add_board( URT6:/dev/ttyS2 DRIVERS + adc #barometer # all available barometer drivers barometer/bmp280 #batt_smbus @@ -24,7 +25,6 @@ px4_add_board( imu/mpu6000 #irlock #lights/blinkm - #lights/oreoled lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 @@ -35,9 +35,6 @@ px4_add_board( #pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - #stm32/tone_alarm #tap_esc #telemetry # all available telemetry drivers telemetry/frsky_telemetry @@ -57,7 +54,7 @@ px4_add_board( land_detector landing_target_estimator load_mon - local_position_estimator + #local_position_estimator logger mavlink mc_att_control @@ -68,7 +65,7 @@ px4_add_board( sih #vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig index 26bfdaec37f2..41d074536ac1 100644 --- a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig +++ b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig @@ -17,7 +17,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F405RG=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y @@ -65,7 +65,7 @@ CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y CONFIG_MMCSD_MULTIBLOCK_DISABLE=y CONFIG_MM_REGIONS=2 -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -82,7 +82,6 @@ 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 CONFIG_NSH_DISABLE_PUT=y CONFIG_NSH_DISABLE_REBOOT=y CONFIG_NSH_DISABLE_TELNETD=y @@ -120,11 +119,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SEM_NNESTPRIO=8 diff --git a/boards/omnibus/f4sd/src/timer_config.c b/boards/omnibus/f4sd/src/timer_config.c index 23783b9b7a0a..615a728522a3 100644 --- a/boards/omnibus/f4sd/src/timer_config.c +++ b/boards/omnibus/f4sd/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index eb5de80e1915..7c65e66a5ee0 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -42,7 +42,7 @@ px4_add_board( temperature_compensation sih #vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #config diff --git a/boards/px4/cannode-v1/default.cmake b/boards/px4/cannode-v1/default.cmake index 4a08ac97b9b8..8075713e59be 100644 --- a/boards/px4/cannode-v1/default.cmake +++ b/boards/px4/cannode-v1/default.cmake @@ -43,7 +43,6 @@ px4_add_board( DRIVERS bootloaders - stm32 uavcannode MODULES diff --git a/boards/px4/esc-v1/default.cmake b/boards/px4/esc-v1/default.cmake index a466be1648c3..394441bed090 100644 --- a/boards/px4/esc-v1/default.cmake +++ b/boards/px4/esc-v1/default.cmake @@ -45,7 +45,6 @@ px4_add_board( DRIVERS bootloaders - stm32 uavcanesc MODULES diff --git a/boards/px4/esc-v1/nuttx-config/nsh/defconfig b/boards/px4/esc-v1/nuttx-config/nsh/defconfig index 1a21100fbc2e..fb6956c3fb94 100644 --- a/boards/px4/esc-v1/nuttx-config/nsh/defconfig +++ b/boards/px4/esc-v1/nuttx-config/nsh/defconfig @@ -61,7 +61,7 @@ CONFIG_MAX_WDOGPARMS=2 CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_NAME_MAX=93 -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=12 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARGCAT=y diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 03dd711a64e9..bff5e297efbe 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -20,6 +20,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc #barometer # all available barometer drivers barometer/ms5611 #batt_smbus @@ -28,7 +29,7 @@ px4_add_board( #differential_pressure # all available differential pressure drivers differential_pressure/ms4525 #distance_sensor # all available distance sensor drivers - distance_sensor/ll40ls + #distance_sensor/ll40ls #distance_sensor/sf0x gps #heater @@ -41,7 +42,6 @@ px4_add_board( #iridiumsbd #irlock #lights/blinkm - #lights/oreoled lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 @@ -55,9 +55,6 @@ px4_add_board( px4fmu px4io #roboclaw - stm32 - stm32/adc - stm32/tone_alarm #tap_esc #telemetry # all available telemetry drivers #test_ppm @@ -87,7 +84,7 @@ px4_add_board( #temperature_compensation vmount vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update @@ -120,7 +117,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #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 diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 1c17a9ded2fc..dd4b2afb92eb 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc #barometer # all available barometer drivers barometer/ms5611 batt_smbus @@ -29,15 +30,12 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/mpu6000 - imu/mpu9250 + #imu/mpu9250 lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 px4fmu px4io - stm32 - stm32/adc - stm32/tone_alarm #telemetry # all available telemetry drivers telemetry/iridiumsbd tone_alarm @@ -59,7 +57,7 @@ px4_add_board( sensors temperature_compensation vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/init/rc.board_defaults b/boards/px4/fmu-v2/init/rc.board_defaults index 702bbd1fc9ca..54cd09248d8b 100644 --- a/boards/px4/fmu-v2/init/rc.board_defaults +++ b/boards/px4/fmu-v2/init/rc.board_defaults @@ -24,5 +24,5 @@ unset BL_FILE if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 96609b237964..bbd792ed65f3 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -14,6 +14,12 @@ hmc5883 -C -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -T 20608 start +# External SPI +ms5611 -S start + +# Internal SPI (auto detect ms5611 or ms5607) +ms5611 -T 0 -s start + set BOARD_FMUV3 0 # V3 build hwtypecmp supports V2|V2M|V30 @@ -59,6 +65,9 @@ then # sensor heating is available, but we disable it for now param set SENS_EN_THERMAL 0 + # External SPI + ms5611 -S start + # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index f8bfd61c737b..bac55b320b9f 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -19,6 +19,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc #barometer # all available barometer drivers barometer/ms5611 #batt_smbus @@ -38,7 +39,6 @@ px4_add_board( #iridiumsbd irlock #lights/blinkm - #lights/oreoled lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 @@ -51,9 +51,6 @@ px4_add_board( pwm_out_sim px4fmu px4io - stm32 - stm32/adc - stm32/tone_alarm #tap_esc #telemetry # all available telemetry drivers #test_ppm @@ -83,7 +80,7 @@ px4_add_board( temperature_compensation vmount #vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 65398e83b14e..a2e2b5cc776f 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -19,6 +19,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc barometer/ms5611 #batt_smbus camera_capture @@ -28,16 +29,13 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/mpu6000 - imu/mpu9250 + #imu/mpu9250 irlock lights/rgbled magnetometer/hmc5883 optical_flow/px4flow px4fmu px4io - stm32 - stm32/adc - stm32/tone_alarm tone_alarm MODULES diff --git a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig index ef97a53e5c91..72c08a713830 100644 --- a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig @@ -27,7 +27,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -93,7 +93,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -138,11 +138,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 76857a5e730c..d47dbe300a92 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -16,6 +16,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc barometer/ms5611 batt_smbus camera_capture @@ -25,15 +26,12 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/mpu6000 - imu/mpu9250 + #imu/mpu9250 lights/rgbled magnetometer/hmc5883 optical_flow/px4flow px4fmu px4io - stm32 - stm32/adc - stm32/tone_alarm tone_alarm MODULES diff --git a/boards/px4/fmu-v2/src/timer_config.c b/boards/px4/fmu-v2/src/timer_config.c index 5adc4260dbc1..492400eb26ae 100644 --- a/boards/px4/fmu-v2/src/timer_config.c +++ b/boards/px4/fmu-v2/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index c2e45fc0671b..e6cca45f1370 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc #barometer # all available barometer drivers barometer/ms5611 #batt_smbus @@ -34,11 +35,10 @@ px4_add_board( imu/l3gd20 imu/lsm303d imu/mpu6000 - imu/mpu9250 + #imu/mpu9250 #iridiumsbd #irlock #lights/blinkm - #lights/oreoled lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 @@ -51,9 +51,6 @@ px4_add_board( #pwm_out_sim px4fmu px4io - stm32 - stm32/adc - stm32/tone_alarm #tap_esc #telemetry # all available telemetry drivers #test_ppm @@ -83,7 +80,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control - #wind_estimator + #airspeed_selector SYSTEMCMDS #bl_update diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 23e1cfd31f68..d32f0458eb4b 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -20,6 +20,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -37,14 +38,12 @@ px4_add_board( imu/icm20948 irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl - lights/pca8574 #optical_flow # all available optical flow drivers optical_flow/px4flow pca9685 @@ -54,9 +53,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -87,7 +83,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -122,7 +118,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v3/init/rc.board_defaults b/boards/px4/fmu-v3/init/rc.board_defaults index 01238b0354ad..4749b503e86b 100644 --- a/boards/px4/fmu-v3/init/rc.board_defaults +++ b/boards/px4/fmu-v3/init/rc.board_defaults @@ -6,5 +6,5 @@ if [ $AUTOCNF = yes ] then - param set SYS_FMU_TASK 0 + fi diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 44de83adba93..e6ce8ed0e824 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -15,6 +15,12 @@ hmc5883 -C -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -T 20608 start +# External SPI +ms5611 -S start + +# Internal SPI (auto detect ms5611 or ms5607) +ms5611 -T 0 -s start + set BOARD_FMUV3 0 # V3 build hwtypecmp supports V2|V2M|V30 @@ -67,6 +73,9 @@ then ak09916 -X -R 6 start fi + # External SPI + ms5611 -S start + # external L3GD20H is rotated 180 degrees yaw l3gd20 -X -R 4 start diff --git a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig index ef97a53e5c91..fb2062439b55 100644 --- a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig @@ -27,7 +27,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -93,7 +93,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -105,7 +105,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -138,11 +137,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig index 83a8b0a5ec74..44feff2fc398 100644 --- a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig @@ -20,7 +20,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -86,7 +86,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -98,7 +98,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -131,11 +130,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 5f9c1e802f91..b85cf8ffea40 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -20,6 +20,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -36,7 +37,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm @@ -45,7 +45,6 @@ px4_add_board( mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow - lights/pca8574 pca9685 protocol_splitter pwm_input @@ -53,9 +52,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -87,7 +83,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -122,7 +118,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v3/src/timer_config.c b/boards/px4/fmu-v3/src/timer_config.c index 5adc4260dbc1..492400eb26ae 100644 --- a/boards/px4/fmu-v3/src/timer_config.c +++ b/boards/px4/fmu-v3/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 2c5af9a197ee..2d6e0c08af53 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -20,6 +20,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -36,8 +37,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm @@ -53,9 +52,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -86,7 +82,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index edb8a95dcb99..3b58cb5c88a0 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -16,6 +16,7 @@ px4_add_board( TEL2:/dev/ttyS2 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -27,7 +28,6 @@ px4_add_board( imu # all available imu drivers irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers @@ -38,9 +38,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -71,7 +69,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -106,7 +104,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v4/init/rc.board_defaults b/boards/px4/fmu-v4/init/rc.board_defaults index 186da5ee378b..716e0ddf2e45 100644 --- a/boards/px4/fmu-v4/init/rc.board_defaults +++ b/boards/px4/fmu-v4/init/rc.board_defaults @@ -11,3 +11,5 @@ then fi +safety_button start + diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index d0f57537b1a9..d44dba215eae 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -22,6 +22,9 @@ qmc5883 -X start # expansion i2c used for BMM150 rotated by 90deg bmm150 -X -R 2 start +# Internal SPI +ms5611 -s start + # For Teal One airframe if param compare SYS_AUTOSTART 4250 then @@ -65,5 +68,3 @@ then # BMI160 internal SPI bus bmi160 start fi - -pmw3901 start diff --git a/boards/px4/fmu-v4/nuttx-config/include/board.h b/boards/px4/fmu-v4/nuttx-config/include/board.h index 98d0ba6274fa..3c1a239dc411 100644 --- a/boards/px4/fmu-v4/nuttx-config/include/board.h +++ b/boards/px4/fmu-v4/nuttx-config/include/board.h @@ -53,7 +53,7 @@ ************************************************************************************/ /* Clocking *************************************************************************/ -/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE. +/* The PX4FMUV4 uses a 24MHz crystal connected to the HSE. * * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: * System Clock source : PLL (HSE) diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index 10f1921d1e05..82ee00b3e1d3 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -27,7 +27,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -93,7 +93,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -105,7 +105,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -138,11 +137,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig index 2791f268982f..ed4f18b97e84 100644 --- a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig @@ -20,7 +20,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -87,7 +87,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -99,7 +99,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -132,11 +131,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index b0271667886c..2ce6a12bf276 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -16,6 +16,7 @@ px4_add_board( TEL2:/dev/ttyS2 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -27,7 +28,6 @@ px4_add_board( imu # all available imu drivers irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers @@ -40,9 +40,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -74,7 +72,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -109,7 +107,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index 86faaeca8c76..a6fe9ee29c2c 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMUv2 internal definitions + * PX4FMUv4 internal definitions */ #pragma once diff --git a/boards/px4/fmu-v4/src/can.c b/boards/px4/fmu-v4/src/can.c index 7234c4eb8d09..bb44863ea806 100644 --- a/boards/px4/fmu-v4/src/can.c +++ b/boards/px4/fmu-v4/src/can.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_can.c + * @file can.c * * Board-specific CAN functions. */ diff --git a/boards/px4/fmu-v4/src/init.c b/boards/px4/fmu-v4/src/init.c index 984932d66374..61aa8f9ba0b8 100644 --- a/boards/px4/fmu-v4/src/init.c +++ b/boards/px4/fmu-v4/src/init.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_init.c + * @file init.c * * PX4FMU-specific early startup code. This file implements the * board_app_initialize() function that is called early by nsh during startup. diff --git a/boards/px4/fmu-v4/src/led.c b/boards/px4/fmu-v4/src/led.c index ddb34e8a5648..12b5a51bed25 100644 --- a/boards/px4/fmu-v4/src/led.c +++ b/boards/px4/fmu-v4/src/led.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu2_led.c + * @file led.c * * PX4FMU LED backend. */ diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c index c2a778b793de..57cd33ec7c25 100644 --- a/boards/px4/fmu-v4/src/timer_config.c +++ b/boards/px4/fmu-v4/src/timer_config.c @@ -32,7 +32,7 @@ ****************************************************************************/ /* - * @file px4fmu_timer_config.c + * @file timer_config.c * * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. * @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/fmu-v4/src/usb.c b/boards/px4/fmu-v4/src/usb.c index ea49d0b52194..ff68631ad6ae 100644 --- a/boards/px4/fmu-v4/src/usb.c +++ b/boards/px4/fmu-v4/src/usb.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4fmu_usb.c + * @file usb.c * * Board-specific USB functions. */ diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 43db1a434366..a0df1389b29f 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -16,6 +16,7 @@ px4_add_board( TEL2:/dev/ttyS2 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -27,7 +28,6 @@ px4_add_board( imu # all available imu drivers irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers @@ -38,9 +38,7 @@ px4_add_board( pwm_out_sim px4fmu rc_input - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -71,7 +69,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -106,7 +104,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #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 diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index ad612123dd1b..c5613d711149 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -19,6 +19,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -35,8 +36,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm @@ -52,9 +51,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -85,7 +81,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -120,7 +116,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index 3edb215d06dd..630720ae24bf 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -21,3 +21,6 @@ qmc5883 -X start # RM3100 rm3100 start + +# Internal SPI +ms5611 -s start diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig index aae494d2e7ec..340345f93500 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig @@ -27,7 +27,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32=y CONFIG_ARCH_CHIP_STM32F469I=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_MEMCPY=y @@ -93,7 +93,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -105,7 +105,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -138,11 +137,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDIO_BLOCKSETUP=y diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index cd6915423f0a..dae2a4d95aa0 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -19,6 +19,7 @@ px4_add_board( TEL4:/dev/ttyS6 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_trigger @@ -34,14 +35,12 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl - lights/pca8574 #optical_flow # all available optical flow drivers optical_flow/px4flow pca9685 @@ -51,9 +50,6 @@ px4_add_board( px4fmu px4io roboclaw - stm32 - stm32/adc - stm32/tone_alarm tap_esc telemetry # all available telemetry drivers test_ppm @@ -85,7 +81,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -120,7 +116,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v4pro/src/timer_config.c b/boards/px4/fmu-v4pro/src/timer_config.c index c2a778b793de..9947d27daffb 100644 --- a/boards/px4/fmu-v4pro/src/timer_config.c +++ b/boards/px4/fmu-v4pro/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/fmu-v5x/rtps.cmake b/boards/px4/fmu-v5/critmonitor.cmake similarity index 84% rename from boards/px4/fmu-v5x/rtps.cmake rename to boards/px4/fmu-v5/critmonitor.cmake index c669ca3036c3..4f90c3bef5a0 100644 --- a/boards/px4/fmu-v5x/rtps.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -2,8 +2,8 @@ px4_add_board( PLATFORM nuttx VENDOR px4 - MODEL fmu-v5x - LABEL rtps + MODEL fmu-v5 + LABEL critmonitor TOOLCHAIN arm-none-eabi ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common @@ -12,13 +12,13 @@ px4_add_board( UAVCAN_INTERFACES 2 SERIAL_PORTS - GPS1:/dev/ttyS1 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS2 - GPS2:/dev/ttyS0 + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -30,31 +30,28 @@ px4_add_board( imu/adis16448 imu/adis16497 #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection -# TBD imu/ism330dlc - needs bus selection + imu/bmi055 imu/mpu6000 + imu/mpu9250 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c + lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl optical_flow # all available optical flow drivers pca9685 power_monitor/ina226 - protocol_splitter + #protocol_splitter pwm_input pwm_out_sim px4fmu px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -70,6 +67,7 @@ px4_add_board( events fw_att_control fw_pos_control_l1 + rover_pos_control land_detector landing_target_estimator load_mon @@ -78,15 +76,13 @@ px4_add_board( mavlink mc_att_control mc_pos_control - micrortps_bridge navigator - rover_pos_control sensors temperature_compensation sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -122,10 +118,10 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 segway uuv_example_app + ) diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 06ec763198db..bf863501a645 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -34,8 +35,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm @@ -52,9 +51,7 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -62,6 +59,7 @@ px4_add_board( uavcan MODULES + airspeed_selector attitude_estimator_q camera_feedback commander @@ -70,7 +68,6 @@ px4_add_board( events fw_att_control fw_pos_control_l1 - rover_pos_control land_detector landing_target_estimator load_mon @@ -80,12 +77,12 @@ px4_add_board( mc_att_control mc_pos_control navigator + rover_pos_control sensors - temperature_compensation sih + temperature_compensation vmount vtol_att_control - wind_estimator SYSTEMCMDS bl_update @@ -121,7 +118,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 275516cea74c..9920fc69dec1 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -17,6 +17,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -36,9 +37,7 @@ px4_add_board( px4fmu px4io rc_input - stm32 - stm32/adc - stm32/tone_alarm + safety_button telemetry # all available telemetry drivers tone_alarm uavcan @@ -57,8 +56,9 @@ px4_add_board( mavlink navigator sensors - temperature_compensation vmount - wind_estimator + temperature_compensation + vmount + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults index df97fadc5cd4..0a44f6e6da1d 100644 --- a/boards/px4/fmu-v5/init/rc.board_defaults +++ b/boards/px4/fmu-v5/init/rc.board_defaults @@ -10,3 +10,5 @@ then fi set LOGGER_BUF 64 + +safety_button start diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index 47db34147b73..4fa0c9151ba9 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -25,5 +25,5 @@ lis3mdl -X start # Possible internal compass ist8310 -C -b 5 start -# Possible pmw3901 optical flow sensor -pmw3901 start +# Baro on internal SPI +ms5611 -s start diff --git a/boards/px4/fmu-v5x/stackcheck.cmake b/boards/px4/fmu-v5/irqmonitor.cmake similarity index 65% rename from boards/px4/fmu-v5x/stackcheck.cmake rename to boards/px4/fmu-v5/irqmonitor.cmake index 05c4838799a0..fb35fc4b9cbd 100644 --- a/boards/px4/fmu-v5x/stackcheck.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -2,23 +2,23 @@ px4_add_board( PLATFORM nuttx VENDOR px4 - MODEL fmu-v5x - LABEL stackcheck + MODEL fmu-v5 + LABEL irqmonitor TOOLCHAIN arm-none-eabi ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common IO px4_io-v2_default TESTING - #UAVCAN_INTERFACES 2 + UAVCAN_INTERFACES 2 SERIAL_PORTS - GPS1:/dev/ttyS1 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS2 - GPS2:/dev/ttyS0 + GPS1:/dev/ttyS0 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -30,15 +30,14 @@ px4_add_board( imu/adis16448 imu/adis16497 #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection -# TBD imu/ism330dlc - needs bus selection + imu/bmi055 imu/mpu6000 - #irlock - #lights/blinkm - #lights/oreoled - lights/pca8574 + imu/mpu9250 + irlock + lights/blinkm lights/rgbled - #lights/rgbled_ncp5623c + lights/rgbled_ncp5623c + lights/rgbled_pwm magnetometer # all available magnetometer drivers #md25 mkblctrl @@ -52,14 +51,12 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm tone_alarm - #uavcan + uavcan MODULES attitude_estimator_q @@ -70,6 +67,7 @@ px4_add_board( events fw_att_control fw_pos_control_l1 + rover_pos_control land_detector landing_target_estimator load_mon @@ -79,13 +77,12 @@ px4_add_board( mc_att_control mc_pos_control navigator - rover_pos_control sensors temperature_compensation sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -116,15 +113,15 @@ px4_add_board( ver EXAMPLES - #bottle_drop # OBC challenge - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test + bottle_drop # OBC challenge + fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + hello + hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav - #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 - #segway - #uuv_example_app + 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 + segway + uuv_example_app + ) diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index b34ef50090dc..18f7eab5e762 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -28,7 +29,6 @@ px4_add_board( imu/mpu6000 irlock lights/blinkm - lights/oreoled lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm @@ -40,9 +40,7 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers tone_alarm @@ -68,7 +66,7 @@ px4_add_board( temperature_compensation sih vmount - wind_estimator + airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig b/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig new file mode 100644 index 000000000000..3c2d89eb18db --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig @@ -0,0 +1,254 @@ +# +# 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_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_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_MATH_H=y +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_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_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_EXCLUDE_BLOCKS=y +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_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_MAX_WDOGPARMS=2 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=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_NFILE_DESCRIPTORS=20 +CONFIG_NFILE_STREAMS=8 +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_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +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_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_PIPES=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PREALLOC_WDOGS=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_CRITMONITOR=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=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=32 +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_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_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM1=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM4=y +CONFIG_STM32F7_TIM9=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_CRITMONITOR=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TIME_EXTENDED=y +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_RXDMA=y +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=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig similarity index 85% rename from boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig rename to boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig index 30d95d6c2021..9d6537c2496b 100644 --- a/boards/px4/fmu-v5x/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig @@ -1,3 +1,10 @@ +# +# 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 @@ -19,7 +26,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32F765II=y CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y @@ -27,7 +34,6 @@ 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_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y @@ -36,12 +42,12 @@ 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_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x3185 -CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y @@ -90,7 +96,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -102,7 +108,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -135,13 +140,15 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_IRQMONITOR=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y +CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SEM_NNESTPRIO=8 CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y @@ -175,22 +182,22 @@ CONFIG_STM32F7_RTC=y CONFIG_STM32F7_RTC_HSECLOCK=y CONFIG_STM32F7_RTC_MAGIC_REG=1 CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC2=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_SPI2=y -CONFIG_STM32F7_SPI3=y CONFIG_STM32F7_SPI4=y CONFIG_STM32F7_SPI5=y CONFIG_STM32F7_SPI6=y CONFIG_STM32F7_TIM10=y CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM1=y CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM4=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 @@ -198,7 +205,9 @@ 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 @@ -206,34 +215,32 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_TIME_EXTENDED=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART5_IFLOWCONTROL=y -CONFIG_UART5_OFLOWCONTROL=y -CONFIG_UART5_RXBUFSIZE=600 -CONFIG_UART5_RXDMA=y -CONFIG_UART5_TXBUFSIZE=1500 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_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_RXDMA=y 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_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 CONFIG_USART3_BAUD=57600 -CONFIG_USART3_RXBUFSIZE=180 -CONFIG_USART3_SERIAL_CONSOLE=y -CONFIG_USART3_TXBUFSIZE=1500 +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 diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 27bb01918d34..b17ac5f06c14 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -26,7 +26,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32F765II=y CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y @@ -96,7 +96,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -108,7 +108,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -141,11 +140,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 96fc19360341..0fa6dc3f3820 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -19,7 +19,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32F765II=y CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y @@ -90,7 +90,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 CONFIG_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -102,7 +102,6 @@ CONFIG_NSH_DISABLE_IFCONFIG=y CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_PSSTACKUSAGE=y CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 @@ -135,11 +134,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SDMMC1_SDIO_MODE=y diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 67b9e60b6046..09838418caf6 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -17,6 +17,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -25,7 +26,6 @@ px4_add_board( gps imu/bmi055 imu/mpu6000 - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm @@ -40,9 +40,7 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button telemetry # all available telemetry drivers tone_alarm uavcan diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index aa3e3581d60b..f0f814df50d1 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -33,8 +34,6 @@ px4_add_board( imu/mpu9250 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm @@ -50,9 +49,7 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -84,7 +81,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -119,7 +116,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index 8f4c3bced423..ce140f86ae64 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -215,6 +215,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_5V_RC_EN(true); VDD_5V_WIFI_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(), @@ -224,8 +228,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } - px4_platform_init(); - /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { diff --git a/boards/px4/fmu-v5/src/timer_config.c b/boards/px4/fmu-v5/src/timer_config.c index bf4db8f412aa..42a838eef23c 100644 --- a/boards/px4/fmu-v5/src/timer_config.c +++ b/boards/px4/fmu-v5/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 0cd950cba1db..66149740d91b 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -18,6 +18,7 @@ px4_add_board( TEL4:/dev/ttyS3 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -33,8 +34,6 @@ px4_add_board( #imu/mpu9250 #irlock #lights/blinkm - #lights/oreoled - lights/pca8574 lights/rgbled #lights/rgbled_ncp5623c lights/rgbled_pwm @@ -42,17 +41,15 @@ px4_add_board( #md25 mkblctrl optical_flow # all available optical flow drivers - pca9685 + #pca9685 #protocol_splitter pwm_input pwm_out_sim px4fmu px4io rc_input - roboclaw - stm32 - stm32/adc - stm32/tone_alarm + #roboclaw + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -84,7 +81,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -120,7 +117,6 @@ px4_add_board( #hello #hwtest # Hardware test #matlab_csv_serial - #position_estimator_inav #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 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index fdaf39476c53..d0a5f705a98f 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -19,6 +19,7 @@ px4_add_board( GPS2:/dev/ttyS0 DRIVERS + adc barometer # all available barometer drivers batt_smbus camera_capture @@ -30,13 +31,11 @@ px4_add_board( imu/adis16448 imu/adis16497 #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection + imu/bmi088 # TBD imu/ism330dlc - needs bus selection imu/mpu6000 irlock lights/blinkm - lights/oreoled - lights/pca8574 lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers @@ -52,9 +51,7 @@ px4_add_board( px4io rc_input roboclaw - stm32 - stm32/adc - stm32/tone_alarm + safety_button tap_esc telemetry # all available telemetry drivers test_ppm @@ -85,7 +82,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS bl_update @@ -121,7 +118,6 @@ px4_add_board( hello hwtest # Hardware test #matlab_csv_serial - position_estimator_inav 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 diff --git a/boards/px4/fmu-v5x/fixedwing.cmake b/boards/px4/fmu-v5x/fixedwing.cmake deleted file mode 100644 index ab1a0f3afe85..000000000000 --- a/boards/px4/fmu-v5x/fixedwing.cmake +++ /dev/null @@ -1,100 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5x - LABEL fixedwing - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - UAVCAN_INTERFACES 2 - - SERIAL_PORTS - GPS1:/dev/ttyS1 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS2 - GPS2:/dev/ttyS0 - - 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 - #heater - imu/adis16448 - imu/adis16497 - #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection -# TBD imu/ism330dlc - needs bus selection - imu/mpu6000 - irlock - lights/blinkm - lights/oreoled - lights/pca8574 - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - power_monitor/ina226 - pwm_input - pwm_out_sim - px4fmu - px4io - rc_input - stm32 - stm32/adc - stm32/tone_alarm - telemetry # all available telemetry drivers - tone_alarm - uavcan - - MODULES - camera_feedback - commander - dataman - ekf2 - events - fw_att_control - fw_pos_control_l1 - land_detector - load_mon - logger - mavlink - navigator - sensors - temperature_compensation - vmount - wind_estimator - - SYSTEMCMDS - bl_update - config - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - shutdown - top - topic_listener - tune_control - usb_connected - ver - ) diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 9343ceebcee8..e818caaf97a7 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -10,3 +10,5 @@ then fi set LOGGER_BUF 64 + +safety_button start diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index c86192a53732..1649e56273bf 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -31,6 +31,3 @@ bmm150 start # Possible internal Barro bmp388 -I start #bmp388 -J start fixme - -# Possible pmw3901 optical flow sensor -pmw3901 start diff --git a/boards/px4/fmu-v5x/multicopter.cmake b/boards/px4/fmu-v5x/multicopter.cmake deleted file mode 100644 index 390706591e42..000000000000 --- a/boards/px4/fmu-v5x/multicopter.cmake +++ /dev/null @@ -1,106 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5x - 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/ttyS1 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS2 - GPS2:/dev/ttyS0 - - DRIVERS - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - gps - #heater - imu/adis16448 - imu/adis16497 - #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection -# TBD imu/ism330dlc - needs bus selection - imu/mpu6000 - irlock - lights/blinkm - lights/oreoled - lights/pca8574 - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - power_monitor/ina226 - pwm_input - pwm_out_sim - px4fmu - px4io - rc_input - roboclaw - stm32 - stm32/adc - stm32/tone_alarm - tap_esc - telemetry # all available telemetry drivers - tone_alarm - uavcan - - MODULES - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_pos_control - navigator - sensors - temperature_compensation - sih - vmount - wind_estimator - - SYSTEMCMDS - bl_update - config - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - shutdown - top - topic_listener - tune_control - usb_connected - ver - ) diff --git a/boards/px4/fmu-v5x/nuttx-config/include/board.h b/boards/px4/fmu-v5x/nuttx-config/include/board.h index 21c382b7ba9e..ac02f6fe6ac8 100644 --- a/boards/px4/fmu-v5x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v5x/nuttx-config/include/board.h @@ -460,6 +460,30 @@ #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 diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 1a93d8b349bd..b4941f2ec4a7 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -13,9 +13,9 @@ # 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_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 @@ -26,7 +26,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP_STM32F765II=y CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=750 +CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_MATH_H=y CONFIG_ARCH_STACKDUMP=y CONFIG_ARMV7M_BASEPRI_WAR=y @@ -56,6 +56,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 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 @@ -81,6 +82,8 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y CONFIG_MAX_TASKS=64 @@ -96,7 +99,25 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y -CONFIG_NFILE_DESCRIPTORS=54 +CONFIG_NFILE_DESCRIPTORS=20 +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSCLIENT_ENTRIES=8 +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_PHY_IOCTL=y +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_NFILE_STREAMS=8 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -104,19 +125,20 @@ 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_PSSTACKUSAGE=y -CONFIG_NSH_DISABLE_TELNETD=y +CONFIG_NSH_DRIPADDR=0XC0A800FE +CONFIG_NSH_IPADDR=0XC0A8007B CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=12 CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_NOMAC=y 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_NXFONTS_DISABLE_16BPP=y CONFIG_NXFONTS_DISABLE_1BPP=y @@ -141,11 +163,11 @@ CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 CONFIG_SCHED_INSTRUMENTATION=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y CONFIG_SEM_NNESTPRIO=8 @@ -162,12 +184,14 @@ CONFIG_START_DAY=30 CONFIG_START_MONTH=11 CONFIG_STDIO_BUFFER_SIZE=32 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 @@ -176,6 +200,11 @@ CONFIG_STM32F7_I2C4=y CONFIG_STM32F7_I2C_DYNTIMEO=y CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PHYSR=16 +CONFIG_STM32F7_PHYSR_100MBPS=0x0 +CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x01 +CONFIG_STM32F7_PHYSR_MODE=0x04 +CONFIG_STM32F7_PHYSR_SPEED=0x2 CONFIG_STM32F7_PWR=y CONFIG_STM32F7_RTC=y CONFIG_STM32F7_RTC_HSECLOCK=y @@ -210,6 +239,7 @@ 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_TIME_EXTENDED=y CONFIG_UART4_BAUD=57600 diff --git a/boards/px4/fmu-v5x/rover.cmake b/boards/px4/fmu-v5x/rover.cmake deleted file mode 100644 index 8b42e8a0baff..000000000000 --- a/boards/px4/fmu-v5x/rover.cmake +++ /dev/null @@ -1,102 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5x - LABEL rover - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - IO px4_io-v2_default - UAVCAN_INTERFACES 2 - - SERIAL_PORTS - GPS1:/dev/ttyS1 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS2 - GPS2:/dev/ttyS0 - - DRIVERS - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - distance_sensor # all available distance sensor drivers - gps - #heater - imu/adis16448 - imu/adis16497 - #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection -# TBD imu/ism330dlc - needs bus selection - imu/mpu6000 - irlock - lights/blinkm - lights/oreoled - lights/pca8574 - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - #md25 - mkblctrl - optical_flow # all available optical flow drivers - pca9685 - power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - px4fmu - px4io - rc_input - roboclaw - stm32 - stm32/adc - stm32/tone_alarm - telemetry # all available telemetry drivers - tone_alarm - uavcan - - MODULES - camera_feedback - commander - dataman - ekf2 - events - land_detector - load_mon - logger - mavlink - navigator - rover_pos_control - sensors - temperature_compensation - vmount - - SYSTEMCMDS - bl_update - config - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - shutdown - top - topic_listener - tune_control - usb_connected - ver - ) diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 1397d988b1ec..030bca52c014 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -318,6 +318,7 @@ #define ADC_ADC1_6V6_CHANNEL /* PC0 */ ADC1_CH(10) #define ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL /* PC2 */ ADC1_CH(12) #define ADC_ADC1_3V3_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) @@ -328,12 +329,16 @@ (1 << ADC_SCALED_V5_CHANNEL) | \ (1 << ADC_ADC1_6V6_CHANNEL) | \ (1 << ADC_SCALED_VDD_3V3_SENSORS4_CHANNEL) | \ - (1 << ADC_ADC1_3V3_CHANNEL) | \ - (1 << ADC_HW_VER_SENSE_CHANNEL) | \ - (1 << ADC_HW_REV_SENSE_CHANNEL)) + (1 << ADC_ADC1_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 + + + #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ @@ -490,6 +495,7 @@ #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) #define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) +#define VDD_3V3_ETH_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_ETH_POWER_EN, (on_true)) /* Tone alarm output */ diff --git a/boards/px4/fmu-v5x/src/init.c b/boards/px4/fmu-v5x/src/init.c index a3544aeda7d3..83f76d19451a 100644 --- a/boards/px4/fmu-v5x/src/init.c +++ b/boards/px4/fmu-v5x/src/init.c @@ -182,6 +182,8 @@ stm32_boardinitialize(void) stm32_usbinitialize(); + VDD_3V3_ETH_POWER_EN(true); + } /**************************************************************************** @@ -222,6 +224,10 @@ __EXPORT int board_app_initialize(uintptr_t arg) VDD_3V3_SENSORS4_EN(true); 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(), @@ -231,7 +237,6 @@ __EXPORT int board_app_initialize(uintptr_t arg) syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); } - px4_platform_init(); /* configure the DMA allocator */ diff --git a/boards/px4/fmu-v5x/src/timer_config.c b/boards/px4/fmu-v5x/src/timer_config.c index 6d70c8686a98..51f07bbf73c1 100644 --- a/boards/px4/fmu-v5x/src/timer_config.c +++ b/boards/px4/fmu-v5x/src/timer_config.c @@ -46,7 +46,7 @@ #include #include -#include +#include #include "board_config.h" diff --git a/boards/px4/io-v2/default.cmake b/boards/px4/io-v2/default.cmake index 0dbbf93cfe72..d1144c1bd494 100644 --- a/boards/px4/io-v2/default.cmake +++ b/boards/px4/io-v2/default.cmake @@ -6,7 +6,6 @@ px4_add_board( TOOLCHAIN arm-none-eabi ARCHITECTURE cortex-m3 DRIVERS - stm32 MODULES px4iofirmware ) diff --git a/boards/px4/io-v2/src/timer_config.c b/boards/px4/io-v2/src/timer_config.c index 7d75d4c5b18c..72968008aac7 100644 --- a/boards/px4/io-v2/src/timer_config.c +++ b/boards/px4/io-v2/src/timer_config.c @@ -42,7 +42,7 @@ #include #include -#include +#include #include diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index df1c98a3ba0d..206eb60e7001 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -48,7 +48,7 @@ px4_add_board( sih vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn @@ -74,7 +74,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 888c3d97cd98..f2e39616116c 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -47,7 +47,7 @@ px4_add_board( #simulator vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS dyn @@ -73,7 +73,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 918eee439a3f..9db93ae25869 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -17,7 +17,6 @@ px4_add_board( #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers - sim/tone_alarm tone_alarm #uavcan @@ -46,7 +45,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update @@ -78,7 +77,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 5aa7fd18e293..6704baab71b5 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -17,7 +17,6 @@ px4_add_board( #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers - sim/tone_alarm tone_alarm #uavcan @@ -47,7 +46,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update @@ -79,7 +78,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/px4/sitl/src/CMakeLists.txt b/boards/px4/sitl/src/CMakeLists.txt index 0e81fc2b7b73..d3156ed0dcdf 100644 --- a/boards/px4/sitl/src/CMakeLists.txt +++ b/boards/px4/sitl/src/CMakeLists.txt @@ -34,4 +34,6 @@ add_library(drivers_board ${PX4_SOURCE_DIR}/src/drivers/boards/common/board_identity.c # TODO: this is horrible and should be fixed sitl_led.c + sitl_board_shutdown.c ) + diff --git a/boards/px4/sitl/src/board_config.h b/boards/px4/sitl/src/board_config.h index ab201f26b238..376d867694a7 100644 --- a/boards/px4/sitl/src/board_config.h +++ b/boards/px4/sitl/src/board_config.h @@ -44,7 +44,7 @@ #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) -#define BOARD_HAS_NO_RESET +#define BOARD_HAS_POWER_CONTROL #define BOARD_HAS_NO_BOOTLOADER #define PX4_I2C_BUS_EXPANSION 1 diff --git a/boards/px4/sitl/src/sitl_board_shutdown.c b/boards/px4/sitl/src/sitl_board_shutdown.c new file mode 100644 index 000000000000..9ca9326a5365 --- /dev/null +++ b/boards/px4/sitl/src/sitl_board_shutdown.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 sitl_board_shutdown.c + * + * sitl board shutdown backend. + */ + +#include +#include + +int board_register_power_state_notification_cb(power_button_state_notification_t cb) +{ + return 0; +} + +int board_shutdown() +{ + px4_systemreset(false); + return 0; +} diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 2ad3006a6cb8..7a7783cc4b95 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -17,7 +17,6 @@ px4_add_board( #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers - sim/tone_alarm tone_alarm #uavcan @@ -46,7 +45,7 @@ px4_add_board( temperature_compensation vmount vtol_att_control - wind_estimator + airspeed_selector SYSTEMCMDS #bl_update @@ -78,7 +77,6 @@ px4_add_board( fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test - position_estimator_inav 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 diff --git a/boards/thiemar/s2740vc-v1/default.cmake b/boards/thiemar/s2740vc-v1/default.cmake index 045206334a64..4c2fa67db98a 100644 --- a/boards/thiemar/s2740vc-v1/default.cmake +++ b/boards/thiemar/s2740vc-v1/default.cmake @@ -36,7 +36,6 @@ px4_add_board( DRIVERS bootloaders - stm32 #uavcannode MODULES diff --git a/boards/thiemar/s2740vc-v1/src/CMakeLists.txt b/boards/thiemar/s2740vc-v1/src/CMakeLists.txt index 1f19644ff933..20a1df3f1c12 100644 --- a/boards/thiemar/s2740vc-v1/src/CMakeLists.txt +++ b/boards/thiemar/s2740vc-v1/src/CMakeLists.txt @@ -41,6 +41,6 @@ add_library(drivers_board ) target_link_libraries(drivers_board - PRIVATE - drivers_arch + PRIVATE + px4_layer ) diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake new file mode 100644 index 000000000000..72dd51004bad --- /dev/null +++ b/boards/uvify/core/default.cmake @@ -0,0 +1,119 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR uvify + MODEL core + LABEL default + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT px4fmu_common + #TESTING + UAVCAN_INTERFACES 1 + + SERIAL_PORTS + GPS1:/dev/ttyS3 + TEL1:/dev/ttyS1 + TEL2:/dev/ttyS2 + + DRIVERS + 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 + #heater + #imu # all available imu drivers + imu/mpu6000 + imu/mpu9250 + irlock + #lights/blinkm + #lights/rgbled + lights/rgbled_ncp5623c + #magnetometer # all available magnetometer drivers + magnetometer/bmm150 + magnetometer/lis3mdl + magnetometer/ist8310 + #mkblctrl + #optical_flow # all available optical flow drivers + optical_flow/px4flow + pca9685 + pwm_input + pwm_out_sim + px4fmu + rc_input + #tap_esc + telemetry # all available telemetry drivers + #test_ppm + tone_alarm + uavcan + + MODULES + attitude_estimator_q + camera_feedback + commander + dataman + ekf2 + events + #fw_att_control + #fw_pos_control_l1 + #rover_pos_control + land_detector + landing_target_estimator + load_mon + local_position_estimator + logger + mavlink + mc_att_control + mc_pos_control + navigator + sensors + sih + vmount + #vtol_att_control + #airspeed_selector + + SYSTEMCMDS + bl_update + config + dmesg + dumpfile + esc_calib + hardfault_log + i2cdetect + led_control + 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 + usb_connected + ver + + 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 + #segway + #uuv_example_app + + ) diff --git a/boards/uvify/core/firmware.prototype b/boards/uvify/core/firmware.prototype new file mode 100644 index 000000000000..88971ff33eee --- /dev/null +++ b/boards/uvify/core/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 20, + "magic": "UVFYCORE", + "description": "Firmware for the UVify Core flight controller", + "image": "", + "build_time": 0, + "summary": "UVify Core", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/uvify/core/init/rc.board_defaults b/boards/uvify/core/init/rc.board_defaults new file mode 100644 index 000000000000..0d7410d11a00 --- /dev/null +++ b/boards/uvify/core/init/rc.board_defaults @@ -0,0 +1,13 @@ +#!/bin/sh +# +# UVify Core specific board defaults +#------------------------------------------------------------------------------ + + +if [ $AUTOCNF = yes ] +then + # Disable safety switch by default + param set CBRK_IO_SAFETY 22027 + +fi + diff --git a/boards/uvify/core/init/rc.board_extras b/boards/uvify/core/init/rc.board_extras new file mode 100644 index 000000000000..4625aa9e3a6e --- /dev/null +++ b/boards/uvify/core/init/rc.board_extras @@ -0,0 +1,18 @@ +#!/bin/sh +# +# UVify UVF4 specific board extras init +#------------------------------------------------------------------------------ + + +# IFO +if param compare SYS_AUTOSTART 4071 +then + # Change rate to 400 Khz for fast barometer + fmu 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. + sleep 4 + ms5611 -T 0 -X start +fi diff --git a/boards/uvify/core/init/rc.board_sensors b/boards/uvify/core/init/rc.board_sensors new file mode 100644 index 000000000000..7c4be4ad2008 --- /dev/null +++ b/boards/uvify/core/init/rc.board_sensors @@ -0,0 +1,41 @@ +#!/bin/sh +# +# UVify UVF4 specific board sensors init +#------------------------------------------------------------------------------ + +# Internal SPI +ms5611 -s start + +# Draco-R +if param compare SYS_AUTOSTART 6002 +then + # Barometer + # Internal SPI + ms5611 -T 0 -s start + + # PX4flow + px4flow start + ist8310 start + mpu6000 -R 2 -T 20608 start + mpu9250 -R 2 start + lis3mdl -R 2 -X start +fi + +# Draco +if param compare SYS_AUTOSTART 4072 +then + mpu9250 -R 2 start +fi + +# IFO +if param compare SYS_AUTOSTART 4071 +then + # IFO GPS LED + rgbled_ncp5623c start -a 0x38 + + # IFO rgb LED + pca9685 start + + mpu9250 -R 2 start + lis3mdl -R 2 -X start +fi diff --git a/boards/uvify/core/nuttx-config/include/board.h b/boards/uvify/core/nuttx-config/include/board.h new file mode 100644 index 000000000000..aa158ba6a209 --- /dev/null +++ b/boards/uvify/core/nuttx-config/include/board.h @@ -0,0 +1,346 @@ +/************************************************************************************ + * 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The UVify Core uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * 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) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * 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 24MHz + * LSE - not installed + */ + +#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 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (25,000,000 / 25) * 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(24) +#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 + +/* DMA Channl/Stream Selections *****************************************************/ +/* Stream selections are arbitrary for now but might become important in the future + * is we set aside more DMA channels/streams. + * + * SDIO DMA + *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA + *   DMAMAP_SDIO_2 = Channel 4, Stream 6 + */ + +#define DMAMAP_SDIO DMAMAP_SDIO_1 + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* ESP8266 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 +#define GPIO_USART2_CTS GPIO_USART2_CTS_2 + +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 +#define GPIO_USART3_CTS GPIO_USART3_CTS_2 + +#define GPIO_UART4_RX GPIO_UART4_RX_1 +#define GPIO_UART4_TX GPIO_UART4_TX_1 + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* RC_INPUT */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 + +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* UART8 has no alternate pin config */ + +/* UART RX DMA configurations */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * CAN + * + * CAN1 is routed to the onboard transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 + +/* + * 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 +#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_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + + +/* + * SPI + * + * There are sensors on SPI1, and SPI2 is connected to the FRAM. + */ +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) + +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_1|GPIO_SPEED_50MHz) + +#if defined(CONFIG_STM32_SPI4) +# define GPIO_SPI4_MISO (GPIO_SPI4_MISO_1|GPIO_SPEED_50MHz) +# define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_1|GPIO_SPEED_50MHz) +# define GPIO_SPI4_SCK (GPIO_SPI4_SCK_1|GPIO_SPEED_50MHz) +#endif + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +# 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); } \ + } 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 + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * 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 void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/uvify/core/nuttx-config/nsh/defconfig b/boards/uvify/core/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..39a594e1f3a1 --- /dev/null +++ b/boards/uvify/core/nuttx-config/nsh/defconfig @@ -0,0 +1,247 @@ +# +# 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_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_STM32_CCMEXCLUDE is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F427V=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_MATH_H=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=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_PRODUCTID=0x0012 +CONFIG_CDCACM_PRODUCTSTR="Core" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=2000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="UVify" +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_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_EXCLUDE_BLOCKS=y +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_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_MAX_WDOGPARMS=2 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_MULTIBLOCK_DISABLE=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_NFILE_DESCRIPTORS=20 +CONFIG_NFILE_STREAMS=8 +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_TELNETD=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +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_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_PIPES=y +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PREALLOC_WDOGS=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=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_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1536 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SDIO_BLOCKSETUP=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=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_SPI2=y +CONFIG_STM32_SPI4=y +CONFIG_STM32_TIM10=y +CONFIG_STM32_TIM11=y +CONFIG_STM32_TIM1=y +CONFIG_STM32_TIM4=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_TIME_EXTENDED=y +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=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=2624 +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/uvify/core/nuttx-config/scripts/script.ld b/boards/uvify/core/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..0d19229c01cf --- /dev/null +++ b/boards/uvify/core/nuttx-config/scripts/script.ld @@ -0,0 +1,159 @@ +/**************************************************************************** + * 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 STM32F427 has 2048Kb 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 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + 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) +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.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = 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/uvify/core/src/CMakeLists.txt b/boards/uvify/core/src/CMakeLists.txt new file mode 100644 index 000000000000..481e75cdcda9 --- /dev/null +++ b/boards/uvify/core/src/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# 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(drivers_board + can.c + init.c + led.c + spi.c + timer_config.c + usb.c +) + +target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/uvify/core/src/board_config.h b/boards/uvify/core/src/board_config.h new file mode 100644 index 000000000000..cb358e1310de --- /dev/null +++ b/boards/uvify/core/src/board_config.h @@ -0,0 +1,389 @@ +/**************************************************************************** + * + * 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 + * + * UVify Core internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ +/* Configuration ************************************************************************************/ + +/* PX4FMU GPIOs ***********************************************************************************/ +/* LEDs */ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3) + +#define GPIO_LED_RED GPIO_LED1 +#define GPIO_LED_GREEN GPIO_LED2 +#define GPIO_LED_BLUE GPIO_LED3 + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_LED LED_BLUE +#define BOARD_ARMED_STATE_LED LED_GREEN + +/** + * The MPU9250 is default. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTC_PIN2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + +/** + * The ICM20608G is default. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTC_PIN15 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + +/** + * Reserved. The wrong driver will fail during start because of an incorrect WHO_AM_I register. + */ +#define GPIO_SPI1_CS_PORTE_PIN15 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + +/* Define the Data Ready interrupts On SPI 1. */ +#define GPIO_DRDY_PORTD_PIN15 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_DRDY_PORTC_PIN14 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14) +#define GPIO_DRDY_PORTE_PIN12 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12) + + +/* Define the Chip Selects for SPI2. */ +#define GPIO_SPI2_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI2_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + +/* Define the Chip Selects for SPI4. */ + +#ifdef CONFIG_STM32_SPI4 +# define BOARD_HAS_BUS_MANIFEST 1 // We support a bus manifest because spi 4 is optional +# define GPIO_SPI4_CS_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) //ESP_RTS_PIN +#endif /* CONFIG_STM32_SPI4 */ +/** + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs. + */ +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz)) + +/* SPI 1 bus off. */ +#define GPIO_SPI1_SCK_OFF _PIN_OFF(GPIO_SPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(GPIO_SPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(GPIO_SPI1_MOSI) + +/* SPI 1 CS's off. */ +#define GPIO_SPI1_CS_OFF_PORTC_PIN2 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN2) +#define GPIO_SPI1_CS_OFF_PORTC_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTC_PIN15) +#define GPIO_SPI1_CS_OFF_PORTE_PIN15 _PIN_OFF(GPIO_SPI1_CS_PORTE_PIN15) + +/* SPI 1 DRDY's off. */ +#define GPIO_DRDY_OFF_PORTD_PIN15 _PIN_OFF(GPIO_DRDY_PORTD_PIN15) +#define GPIO_DRDY_OFF_PORTC_PIN14 _PIN_OFF(GPIO_DRDY_PORTC_PIN14) +#define GPIO_DRDY_OFF_PORTE_PIN12 _PIN_OFF(GPIO_DRDY_PORTE_PIN12) + +/* SPI 4 bus off. */ +#ifdef CONFIG_STM32_SPI4 +# define GPIO_SPI4_SCK_OFF _PIN_OFF(GPIO_SPI4_SCK) +# define GPIO_SPI4_MISO_OFF _PIN_OFF(GPIO_SPI4_MISO) +# define GPIO_SPI4_MOSI_OFF _PIN_OFF(GPIO_SPI4_MOSI) +#endif /* CONFIG_STM32_SPI4 */ + +/** + * N.B we do not have control over the SPI 2 buss powered devices + * so the the ms5611 is not resetable. + * + */ + +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_RAMTRON 2 +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_RAMTRON + +#ifdef CONFIG_STM32_SPI4 +# define PX4_SPI_BUS_EXTERNAL 4 +/* The mask passes to init the SPI bus pins + * N.B This works ONLY with buss numbers that are powers of 2 + * Adding SPI3 would break this! + */ +# define SPI_BUS_INIT_MASK_EXT PX4_SPI_BUS_EXTERNAL +#endif /* CONFIG_STM32_SPI4 */ + +#define SPI_BUS_INIT_MASK (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_SENSORS) + +/* Use these in place of the uint32_t enumeration to select a specific SPI device on SPI1 */ +#define PX4_SPIDEV_GYRO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) +#define PX4_SPIDEV_ACCEL_MAG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 4) +#define PX4_SPIDEV_HMC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 5) +#define PX4_SPIDEV_ICM PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 6) +#define PX4_SPIDEV_LIS PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 7) +#define PX4_SPIDEV_BMI PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 8) +#define PX4_SPIDEV_BMA PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 9) +#define PX4_SPIDEV_ICM_20608 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 10) +#define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 11) +#define PX4_SPIDEV_BMI055_ACC PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 12) +#define PX4_SPIDEV_BMI055_GYR PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 13) +#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 14) + +/** + * Onboard MS5611 and FRAM are both on bus SPI2. + * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver. + * PX4_MK_SPI_SEL differentiate by adding in PX4_SPI_DEVICE_ID. + */ +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_BARO, 3) + +#ifdef CONFIG_STM32_SPI4 +# define PX4_SPIDEV_EXTERNAL PX4_MK_SPI_SEL(PX4_SPI_BUS_EXTERNAL, 1) +#endif /* CONFIG_STM32_SPI4 */ + +/* I2C busses. */ +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_I2C_BUS_LED PX4_I2C_BUS_EXPANSION + +/** + * Devices on the external bus. + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_BMP280 0x76 + +/** + * 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 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) + +/* ADC defines to be used in sensors.cpp to read from a particular channel. */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#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) + + +/** + * User GPIOs: + * GPIO0-5 are the PWM servo outputs. + */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) +#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + +/* 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) +#define GPIO_VDD_3V3_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) + +/* Tone alarm output. */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) + +/** + * PWM: + * + * Six PWM outputs are configured. + * + * Pins: + * + * CH1 : PE14 : TIM1_CH4 + * CH2 : PE13 : TIM1_CH3 + * CH3 : PE11 : TIM1_CH2 + * CH4 : PE9 : TIM1_CH1 + * CH5 : PD13 : TIM4_CH2 + * CH6 : PD14 : TIM4_CH3 + */ + +/** + * N.B. the added pull down, on the timer being disabled the PD + * will keep the channel low + */ +#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) +#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11) +#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13) +#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN14) +#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13) +#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14) +#define DIRECT_PWM_OUTPUT_CHANNELS 6 + +#define GPIO_TIM1_CH1IN GPIO_TIM1_CH1IN_2 +#define GPIO_TIM1_CH2IN GPIO_TIM1_CH2IN_2 +#define GPIO_TIM1_CH3IN GPIO_TIM1_CH3IN_2 +#define GPIO_TIM1_CH4IN GPIO_TIM1_CH4IN_2 +#define GPIO_TIM4_CH2IN GPIO_TIM4_CH2IN_2 +#define GPIO_TIM4_CH3IN GPIO_TIM4_CH3IN_2 +#define DIRECT_INPUT_TIMER_CHANNELS 6 + +/** + * USB OTG FS: + * PA9 OTG_FS_VBUS VBUS sensing. + */ +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) + +/* 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_PPM_CHANNEL 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS4" + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2. */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL 2 +#define GPIO_PWM_IN GPIO_TIM4_CH2IN_2 + +#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN3) +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) + +/* For,this signal is active high. */ +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true) + +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) + +/* GPIOs */ +#define GPIO_PE2 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) +#define GPIO_PB4 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4) +#define GPIO_PE5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +#define GPIO_PE6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + +/* 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) + +/* Power switch controls */ + +#define SPEKTRUM_POWER(_on_true) px4_arch_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (!_on_true)) + +/** + * UVify Core has separate RC_IN + * + * GPIO PPM_IN on PB0 T3C3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible via the 74LVC2G86 controlled by the FMU + * The FMU 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_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK_VALID)) +#define BOARD_ADC_USB_VALID (px4_arch_gpioread(GPIO_VDD_USB_VALID)) +#define BOARD_ADC_SERVO_VALID (1) +#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_ENABLE_CONSOLE_BUFFER +#define BOARD_CONSOLE_BUFFER_SIZE (1024*3) + +__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. + * + * mask - is bus selection + * 1 - 1 << 0 + * 2 - 1 << 1 + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(int mask); +void board_spi_reset(int ms); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/uvify/core/src/can.c b/boards/uvify/core/src/can.c new file mode 100644 index 000000000000..2835d99a9891 --- /dev/null +++ b/boards/uvify/core/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * 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 can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "up_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/uvify/core/src/init.c b/boards/uvify/core/src/init.c new file mode 100644 index 000000000000..131f883ad3c2 --- /dev/null +++ b/boards/uvify/core/src/init.c @@ -0,0 +1,403 @@ +/**************************************************************************** + * + * 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 init.c + * + * UVify Core 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 + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/** + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void 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) +{ + // Set the peripheral rails off. + stm32_configgpio(GPIO_PERIPH_3V3_EN); + + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); + + bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN); + // Keep Spektum on to discharge rail. + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); + + // 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. + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); + +} + +/************************************************************************************ + * 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. + stm32_configgpio(GPIO_GPIO0_OUTPUT); + stm32_configgpio(GPIO_GPIO1_OUTPUT); + stm32_configgpio(GPIO_GPIO2_OUTPUT); + stm32_configgpio(GPIO_GPIO3_OUTPUT); + stm32_configgpio(GPIO_GPIO4_OUTPUT); + stm32_configgpio(GPIO_GPIO5_OUTPUT); + + /** + * 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_IN2); /* BATT_VOLTAGE_SENS */ + stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ + stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ + stm32_configgpio(GPIO_ADC1_IN11); /* RSSI analog in */ + + // Configure CAN interface + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + + // Configure power supply control/sense pins. + stm32_configgpio(GPIO_PERIPH_3V3_EN); + stm32_configgpio(GPIO_VDD_BRICK_VALID); + stm32_configgpio(GPIO_VDD_USB_VALID); + + /** + * Start with Sensor voltage off We will enable it + * in board_app_initialize. + */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + + stm32_configgpio(GPIO_SBUS_INV); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); + + stm32_configgpio(GPIO_PB4); + stm32_configgpio(GPIO_PE2); + + // Safety - led on in led driver. + stm32_configgpio(GPIO_BTN_SAFETY); + stm32_configgpio(GPIO_PPM_IN); + + int spi_init_mask = SPI_BUS_INIT_MASK; + +#if defined(CONFIG_STM32_SPI4) + + /* We have SPI4 is GPIO_PB4 pin 3 Low */ + if (stm32_gpioread(GPIO_PB4) == 0) { + spi_init_mask |= SPI_BUS_INIT_MASK_EXT; + + } else { +#endif /* CONFIG_STM32_SPI4 */ + + stm32_configgpio(GPIO_PE5); + stm32_configgpio(GPIO_PE6); + +#if defined(CONFIG_STM32_SPI4) + } + +#endif /* CONFIG_STM32_SPI4 */ + +// Configure SPI all interfaces GPIO. + stm32_spiinitialize(spi_init_mask); + + // Configure heater GPIO. + stm32_configgpio(GPIO_HEATER_INPUT); + stm32_configgpio(GPIO_HEATER_OUTPUT); +} + +/**************************************************************************** + * 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 sdio_dev_s *sdio; +#if defined(CONFIG_STM32_SPI4) +static struct spi_dev_s *spi4; +#endif + +__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"); + } + + // Set up the serial DMA polling. + 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_off(LED_RED); + led_off(LED_GREEN); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + + // Power up the sensors. + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + // Power down the heater. + stm32_gpiowrite(GPIO_HEATER_OUTPUT, 0); + + // Configure SPI-based devices. + spi1 = stm32_spibus_initialize(1); + + if (!spi1) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n"); + led_on(LED_RED); + 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); + SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi1, PX4_SPIDEV_HMC, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); + up_udelay(20); + + // Get the SPI port for the FRAM. + spi2 = stm32_spibus_initialize(2); + + if (!spi2) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n"); + led_on(LED_RED); + return -ENODEV; + } + + /** + * Default SPI2 to 12MHz and de-assert the known chip selects. + * MS5611 has max SPI clock speed of 20MHz. + */ + + // XXX start with 10.4 MHz and go up to 20 once validated. + SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, SPIDEV_FLASH(0), false); + SPI_SELECT(spi2, PX4_SPIDEV_BARO, false); + +#if defined(CONFIG_STM32_SPI4) + + if (stm32_gpioread(GPIO_PB4) == 0) { + syslog(LOG_INFO, "[boot] GPIO_PB4 - Low Initialize SPI port 4 \n"); + + // Configure SPI-based devices. + spi4 = stm32_spibus_initialize(4); + + if (!spi4) { + syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 4\n"); + + } else { + // Default SPI4 to 20 MHz and de-assert the known chip selects. + SPI_SETFREQUENCY(spi4, 20 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXTERNAL, false); + } + } + +#endif /* defined(CONFIG_STM32_SPI4) */ + + +#ifdef CONFIG_MMCSD + + // First, get an instance of the SDIO interface. + sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdio) { + led_on(LED_RED); + 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. + int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + + 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. + sdio_mediachange(sdio, true); + +#endif + + return OK; +} diff --git a/boards/uvify/core/src/led.c b/boards/uvify/core/src/led.c new file mode 100644 index 000000000000..d088d75c4ed8 --- /dev/null +++ b/boards/uvify/core/src/led.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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 led.c + * + * UVify Core LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void 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, // Indexed by LED_BLUE + GPIO_LED_RED, // Indexed by LED_RED, LED_AMBER + GPIO_LED_SAFETY, // Indexed by LED_SAFETY + GPIO_LED_GREEN, // Indexed by LED_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++) { + stm32_configgpio(g_ledmap[l]); + } +} + +static void phy_set_led(int led, bool state) +{ + /* Pull Down to switch on */ + stm32_gpiowrite(g_ledmap[led], !state); +} + +static bool phy_get_led(int led) +{ + + return !stm32_gpioread(g_ledmap[led]); +} + +__EXPORT void led_on(int led) +{ + phy_set_led(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/uvify/core/src/spi.c b/boards/uvify/core/src/spi.c new file mode 100644 index 000000000000..dfac2a241256 --- /dev/null +++ b/boards/uvify/core/src/spi.c @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * 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 spi.c + * + * UVify Core specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +__EXPORT bool board_has_bus(enum board_bus_types type, uint32_t bus) +{ + bool rv = true; + + switch (type) { + case BOARD_SPI_BUS: +#ifdef CONFIG_STM32_SPI4 + rv = bus != PX4_SPI_BUS_EXTERNAL || (stm32_gpioread(GPIO_PB4) == 0); +#endif /* CONFIG_STM32_SPI4 */ + break; + + case BOARD_I2C_BUS: + break; + } + + return rv; +} + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * mask - is bus selection + * 1 - 1 << 0 + * 2 - 1 << 1 + * + ************************************************************************************/ + +__EXPORT void stm32_spiinitialize(int mask) +{ +#ifdef CONFIG_STM32_SPI1 + + if (mask & PX4_SPI_BUS_SENSORS) { + stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN2); + stm32_configgpio(GPIO_SPI1_CS_PORTC_PIN15); + stm32_configgpio(GPIO_SPI1_CS_PORTE_PIN15); + + stm32_configgpio(GPIO_DRDY_PORTD_PIN15); + stm32_configgpio(GPIO_DRDY_PORTC_PIN14); + stm32_configgpio(GPIO_DRDY_PORTE_PIN12); + } + +#endif + +#ifdef CONFIG_STM32_SPI2 + + if (mask & (PX4_SPI_BUS_RAMTRON | PX4_SPI_BUS_BARO)) { + stm32_configgpio(GPIO_SPI2_CS_MS5611); + stm32_configgpio(GPIO_SPI2_CS_FRAM); + } + +#endif + +#ifdef CONFIG_STM32_SPI4 + + if (mask & PX4_SPI_BUS_EXTERNAL) { + stm32_configgpio(GPIO_SPI4_CS_1); //add cs + } + +#endif /* CONFIG_STM32_SPI4 */ +} + +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + + /* Shared PC2 CS devices */ + + case PX4_SPIDEV_BMI: + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, !selected); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); + break; + + /* Shared PC15 CS devices */ + + case PX4_SPIDEV_ICM: + case PX4_SPIDEV_ICM_20602: + case PX4_SPIDEV_ICM_20608: + case PX4_SPIDEV_BMI055_ACC: + case PX4_SPIDEV_MPU2: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, !selected); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, 1); + break; + + /* Shared PE15 CS devices */ + + case PX4_SPIDEV_HMC: + case PX4_SPIDEV_LIS: + case PX4_SPIDEV_BMI055_GYR: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN2, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTC_PIN15, 1); + px4_arch_gpiowrite(GPIO_SPI1_CS_PORTE_PIN15, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} + + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case SPIDEV_FLASH(0): + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI2_CS_MS5611, 1); + stm32_gpiowrite(GPIO_SPI2_CS_FRAM, !selected); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI2_CS_FRAM, 1); + stm32_gpiowrite(GPIO_SPI2_CS_MS5611, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + if (devid == PX4_SPIDEV_EXTERNAL && stm32_gpioread(GPIO_PB4) == 0) { + stm32_gpiowrite(GPIO_SPI4_CS_1, !selected); // add cs + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif /* CONFIG_STM32_SPI4 */ + +__EXPORT void board_spi_reset(int ms) +{ + /* disable SPI bus 1 DRDY */ + + stm32_configgpio(GPIO_DRDY_OFF_PORTD_PIN15); + stm32_configgpio(GPIO_DRDY_OFF_PORTC_PIN14); + stm32_configgpio(GPIO_DRDY_OFF_PORTE_PIN12); + + stm32_gpiowrite(GPIO_DRDY_OFF_PORTD_PIN15, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_PORTC_PIN14, 0); + stm32_gpiowrite(GPIO_DRDY_OFF_PORTE_PIN12, 0); + + /* disable SPI bus 1 CS */ + + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN2); + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTC_PIN15); + stm32_configgpio(GPIO_SPI1_CS_OFF_PORTE_PIN15); + + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN2, 0); + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTC_PIN15, 0); + stm32_gpiowrite(GPIO_SPI1_CS_OFF_PORTE_PIN15, 0); + + /* disable SPI bus 1*/ + + stm32_configgpio(GPIO_SPI1_SCK_OFF); + stm32_configgpio(GPIO_SPI1_MISO_OFF); + stm32_configgpio(GPIO_SPI1_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); + +#ifdef CONFIG_STM32_SPI4 + + /* disable SPI bus 4*/ + if (stm32_gpioread(GPIO_PB4) == 0) { + stm32_configgpio(GPIO_SPI4_SCK_OFF); + stm32_configgpio(GPIO_SPI4_MISO_OFF); + stm32_configgpio(GPIO_SPI4_MOSI_OFF); + + stm32_gpiowrite(GPIO_SPI4_SCK_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MISO_OFF, 0); + stm32_gpiowrite(GPIO_SPI4_MOSI_OFF, 0); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* N.B we do not have control over the SPI 2 buss powered devices + * so the the ms5611 is not resetable. + */ + + /* set the sensor rail off (default) */ + stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + /* set the periph rail off (default) for SPI4 */ + stm32_configgpio(GPIO_PERIPH_3V3_EN); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + /* switch the periph rail back on */ + stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); + } + +#endif /* CONFIG_STM32_SPI4 */ + + /* switch the sensor rail back on */ + stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + stm32_spiinitialize(PX4_SPI_BUS_SENSORS); + stm32_configgpio(GPIO_SPI1_SCK); + stm32_configgpio(GPIO_SPI1_MISO); + stm32_configgpio(GPIO_SPI1_MOSI); + +#ifdef CONFIG_STM32_SPI4 + + if (stm32_gpioread(GPIO_PB4) == 0) { + stm32_spiinitialize(PX4_SPI_BUS_EXTERNAL); + stm32_configgpio(GPIO_SPI4_SCK); + stm32_configgpio(GPIO_SPI4_MISO); + stm32_configgpio(GPIO_SPI4_MOSI); + } + +#endif /* CONFIG_STM32_SPI4 */ +} diff --git a/boards/uvify/core/src/timer_config.c b/boards/uvify/core/src/timer_config.c new file mode 100644 index 000000000000..f56750432494 --- /dev/null +++ b/boards/uvify/core/src/timer_config.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 timer_config.c + * + * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN, + .first_channel_index = 0, + .last_channel_index = 3, + .handler = io_timer_handler0, + .vectorno = STM32_IRQ_TIM1CC, + + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN, + .first_channel_index = 4, + .last_channel_index = 5, + .handler = io_timer_handler1, + .vectorno = STM32_IRQ_TIM4, + } +}; + +__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + { + .gpio_out = GPIO_TIM1_CH4OUT, + .gpio_in = GPIO_TIM1_CH4IN, + .timer_index = 0, + .timer_channel = 4, + .ccr_offset = STM32_GTIM_CCR4_OFFSET, + .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF + }, + { + .gpio_out = GPIO_TIM1_CH3OUT, + .gpio_in = GPIO_TIM1_CH3IN, + .timer_index = 0, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + }, + { + .gpio_out = GPIO_TIM1_CH2OUT, + .gpio_in = GPIO_TIM1_CH2IN, + .timer_index = 0, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM1_CH1OUT, + .gpio_in = GPIO_TIM1_CH1IN, + .timer_index = 0, + .timer_channel = 1, + .ccr_offset = STM32_GTIM_CCR1_OFFSET, + .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF + }, + { + .gpio_out = GPIO_TIM4_CH2OUT, + .gpio_in = GPIO_TIM4_CH2IN, + .timer_index = 1, + .timer_channel = 2, + .ccr_offset = STM32_GTIM_CCR2_OFFSET, + .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF + }, + { + .gpio_out = GPIO_TIM4_CH3OUT, + .gpio_in = GPIO_TIM4_CH3IN, + .timer_index = 1, + .timer_channel = 3, + .ccr_offset = STM32_GTIM_CCR3_OFFSET, + .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF + } +}; diff --git a/boards/uvify/core/src/usb.c b/boards/uvify/core/src/usb.c new file mode 100644 index 000000000000..fe85fd61c63a --- /dev/null +++ b/boards/uvify/core/src/usb.c @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * 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 usb.c + * + * UVify Core 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 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_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/cmake/bloaty.cmake b/cmake/bloaty.cmake new file mode 100644 index 000000000000..9f501de7c110 --- /dev/null +++ b/cmake/bloaty.cmake @@ -0,0 +1,90 @@ +############################################################################ +# +# 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. +# +############################################################################ + +find_program(BLOATY_PROGRAM bloaty) +if (BLOATY_PROGRAM) + + set(BLOATY_OPTS --demangle=short --domain=vm -s vm -n 100 -w) + + # bloaty compilation units + add_custom_target(bloaty_compileunits + COMMAND ${BLOATY_PROGRAM} -d compileunits ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + + # bloaty inlines + add_custom_target(bloaty_inlines + COMMAND ${BLOATY_PROGRAM} -d inlines ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + + # bloaty sections + add_custom_target(bloaty_sections + COMMAND ${BLOATY_PROGRAM} -d sections ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + + # bloaty segments + add_custom_target(bloaty_segments + COMMAND ${BLOATY_PROGRAM} -d segments ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + + # bloaty symbols + add_custom_target(bloaty_symbols + COMMAND ${BLOATY_PROGRAM} -d symbols ${BLOATY_OPTS} $ + DEPENDS px4 + USES_TERMINAL + ) + + # bloaty templates + add_custom_target(bloaty_templates + COMMAND ${BLOATY_PROGRAM} -d shortsymbols,fullsymbols ${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 ${BLOATY_PROGRAM} -d symbols ${BLOATY_OPTS} $ -- master.elf + DEPENDS px4 + WORKING_DIRECTORY ${PX4_BINARY_DIR} + VERBATIM + USES_TERMINAL + ) +endif() diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 4ae03387db20..8507b0d2fc1b 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -39,12 +39,12 @@ include(px4_base) # # Adds a googletest unit test to the test_results target. # -function(px4_add_gtest) +function(px4_add_unit_gtest) # skip if unit testing is not configured if(BUILD_TESTING) # parse source file and library dependencies from arguments px4_parse_function_args( - NAME px4_add_gtest + NAME px4_add_unit_gtest ONE_VALUE SRC MULTI_VALUE LINKLIBS REQUIRED SRC @@ -62,7 +62,56 @@ function(px4_add_gtest) target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_main) # add the test to the ctest plan - add_test(NAME ${TESTNAME} COMMAND ${TESTNAME}) + add_test(NAME ${TESTNAME} + COMMAND ${TESTNAME} + WORKING_DIRECTORY ${PX4_BINARY_DIR}) + + # attach it to the unit test target + add_dependencies(test_results ${TESTNAME}) + endif() +endfunction() + +function(px4_add_functional_gtest) + # skip if unit testing is not configured + if(BUILD_TESTING) + # parse source file and library dependencies from arguments + px4_parse_function_args( + NAME px4_add_functional_gtest + ONE_VALUE SRC + MULTI_VALUE LINKLIBS + REQUIRED SRC + ARGN ${ARGN}) + + # infer test name from source filname + get_filename_component(TESTNAME ${SRC} NAME_WE) + string(REPLACE Test "" TESTNAME ${TESTNAME}) + set(TESTNAME functional-${TESTNAME}) + + # build a binary for the unit test + add_executable(${TESTNAME} EXCLUDE_FROM_ALL ${SRC}) + + # link the libary to test and gtest + target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_functional_main + px4_daemon + px4_platform + modules__uORB + px4_layer + systemlib + cdev + px4_work_queue + px4_daemon + work_queue + parameters + perf + tinybson + uorb_msgs + test_stubs) #put test_stubs last + + # add the test to the ctest plan + add_test(NAME ${TESTNAME} + # functional tests need to run in a new process for each test, + # since they set up and tear down system components + COMMAND ${PX4_BINARY_DIR}/${TESTNAME}) # attach it to the unit test target add_dependencies(test_results ${TESTNAME}) diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index 80d9435034df..0c35b1cb6cc6 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -183,6 +183,9 @@ function(px4_add_board) 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() @@ -261,8 +264,8 @@ function(px4_add_board) foreach(driver ${DF_DRIVERS}) list(APPEND config_df_driver_list ${driver}) - if(EXISTS "${PX4_SOURCE_DIR}/src/platforms/posix/drivers/df_${driver}_wrapper") - list(APPEND config_module_list platforms/posix/drivers/df_${driver}_wrapper) + if(EXISTS "${PX4_SOURCE_DIR}/src/drivers/driver_framework_wrapper/df_${driver}_wrapper") + list(APPEND config_module_list drivers/driver_framework_wrapper/df_${driver}_wrapper) endif() endforeach() set(config_df_driver_list ${config_df_driver_list} PARENT_SCOPE) diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index faa7e3c9f3fb..4f5507bf5a38 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -51,6 +51,7 @@ function(px4_add_common_flags) -fdata-sections -ffunction-sections -fomit-frame-pointer + -fmerge-all-constants #-funsafe-math-optimizations # Enables -fno-signed-zeros, -fno-trapping-math, -fassociative-math and -freciprocal-math -fno-signed-zeros # Allow optimizations for floating-point arithmetic that ignore the signedness of zero @@ -87,7 +88,6 @@ function(px4_add_common_flags) -Wunused-variable # disabled warnings - -Wno-implicit-fallthrough # set appropriate level and update -Wno-missing-field-initializers -Wno-missing-include-dirs # TODO: fix and enable -Wno-unused-parameter @@ -175,6 +175,9 @@ function(px4_add_common_flags) ${PX4_BINARY_DIR}/src/lib ${PX4_BINARY_DIR}/src/modules + ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/${PX4_CHIP_MANUFACTURER}/${PX4_CHIP}/include + ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include + ${PX4_SOURCE_DIR}/platforms/common/include ${PX4_SOURCE_DIR}/src ${PX4_SOURCE_DIR}/src/include ${PX4_SOURCE_DIR}/src/lib @@ -182,7 +185,6 @@ function(px4_add_common_flags) ${PX4_SOURCE_DIR}/src/lib/matrix ${PX4_SOURCE_DIR}/src/modules ${PX4_SOURCE_DIR}/src/platforms - ${PX4_SOURCE_DIR}/src/platforms/common ) add_definitions( diff --git a/cmake/px4_add_library.cmake b/cmake/px4_add_library.cmake index a7290fd4823b..2a6377c71a2d 100644 --- a/cmake/px4_add_library.cmake +++ b/cmake/px4_add_library.cmake @@ -40,7 +40,7 @@ include(px4_base) # Like add_library but with PX4 platform dependencies # function(px4_add_library target) - add_library(${target} ${ARGN}) + add_library(${target} EXCLUDE_FROM_ALL ${ARGN}) target_compile_definitions(${target} PRIVATE MODULE_NAME="${target}") diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index e995cb2dfbdd..b0c1b160fe87 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -42,7 +42,6 @@ include(px4_base) # Usage: # px4_add_module(MODULE # MAIN -# [ STACK ] !!!!!DEPRECATED, USE STACK_MAIN INSTEAD!!!!!!!!! # [ STACK_MAIN ] # [ STACK_MAX ] # [ COMPILE_FLAGS ] @@ -87,7 +86,7 @@ function(px4_add_module) px4_parse_function_args( NAME px4_add_module - ONE_VALUE MODULE MAIN STACK STACK_MAIN STACK_MAX PRIORITY + ONE_VALUE MODULE MAIN STACK_MAIN STACK_MAX PRIORITY MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS MODULE_CONFIG OPTIONS EXTERNAL DYNAMIC UNITY_BUILD REQUIRED MODULE MAIN @@ -166,7 +165,7 @@ function(px4_add_module) # set defaults if not set set(MAIN_DEFAULT MAIN-NOTFOUND) - set(STACK_MAIN_DEFAULT 1024) + set(STACK_MAIN_DEFAULT 2048) set(PRIORITY_DEFAULT SCHED_PRIORITY_DEFAULT) foreach(property MAIN STACK_MAIN PRIORITY) diff --git a/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan b/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan new file mode 100644 index 000000000000..f2593118bae8 --- /dev/null +++ b/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan @@ -0,0 +1,107 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": null, + "Altitude": 18, + "AltitudeMode": 1, + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 15, + 0, + 0, + null, + 47.3977394, + 8.5455942, + 18 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 18, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.3977394, + 8.5456982, + 18 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 18, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.3977432, + 8.545805, + 18 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": null, + "Altitude": 0, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.3977432, + 8.545804, + 0 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.3977419, + 8.5455941, + 489 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +} diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 18cf6ff2fc0e..ac40c0329e88 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 18cf6ff2fc0e51e4555b19fc31e8b06eb38bdd79 +Subproject commit ac40c0329e88b70ae5db4c1467ed5853d305af54 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 986865d45ba5..e112e116a96a 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -38,6 +38,7 @@ set(msg_files actuator_outputs.msg adc_report.msg airspeed.msg + airspeed_validated.msg battery_status.msg camera_capture.msg camera_trigger.msg @@ -104,6 +105,7 @@ set(msg_files sensor_combined.msg sensor_correction.msg sensor_gyro.msg + sensor_gyro_control.msg sensor_mag.msg sensor_preflight.msg sensor_selection.msg @@ -122,7 +124,9 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg + vehicle_acceleration.msg vehicle_air_data.msg + vehicle_angular_velocity.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg vehicle_command.msg @@ -142,6 +146,7 @@ set(msg_files vehicle_status_flags.msg vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg + wheel_encoders.msg wind_estimate.msg ) diff --git a/msg/airspeed_validated.msg b/msg/airspeed_validated.msg new file mode 100644 index 000000000000..7b451d7b3c94 --- /dev/null +++ b/msg/airspeed_validated.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid +float32 equivalent_airspeed_m_s # equivalent airspeed in m/s (accounts for instrumentation errors) (EAS), set to NAN if invalid +float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid + +float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid +float32 indicated_ground_minus_wind_m_s # IAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid + +int8 selected_airspeed_index # 0-2: airspeed sensor index, -1: groundspeed-windspeed, -2: airspeed invalid diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 898326287f10..738732436a51 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -7,7 +7,7 @@ float32 average_current_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 temperature # temperature of the battery +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 bool system_source # Whether or not a this battery is the active power source for VDD_5V_IN diff --git a/msg/esc_status.msg b/msg/esc_status.msg index 83a485d19a4d..cb5ea77d4121 100644 --- a/msg/esc_status.msg +++ b/msg/esc_status.msg @@ -17,4 +17,14 @@ uint16 counter # incremented by the writing thread everytime new data is s uint8 esc_count # number of connected ESCs uint8 esc_connectiontype # how ESCs connected to the system +uint8 esc_online_flags # Bitmask indicating which ESC is online/offline +# esc_online_flags bit 0 : Set to 1 if ESC0 is online +# esc_online_flags bit 1 : Set to 1 if ESC1 is online +# esc_online_flags bit 2 : Set to 1 if ESC2 is online +# esc_online_flags bit 3 : Set to 1 if ESC3 is online +# esc_online_flags bit 4 : Set to 1 if ESC4 is online +# esc_online_flags bit 5 : Set to 1 if ESC5 is online +# esc_online_flags bit 6 : Set to 1 if ESC6 is online +# esc_online_flags bit 7 : Set to 1 if ESC7 is online + esc_report[8] esc diff --git a/msg/gps_dump.msg b/msg/gps_dump.msg index 3718e457cb2c..26de92e833fa 100644 --- a/msg/gps_dump.msg +++ b/msg/gps_dump.msg @@ -6,3 +6,5 @@ uint64 timestamp # time since system start (microseconds) uint8 len # length of data, MSB bit set = message to the gps device, # clear = message from the device uint8[79] data # data to write to the log + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg index 00b1aa1b8abe..5daae0623ea2 100644 --- a/msg/gps_inject_data.msg +++ b/msg/gps_inject_data.msg @@ -2,3 +2,5 @@ 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) + +uint8 ORB_QUEUE_LENGTH = 6 diff --git a/msg/led_control.msg b/msg/led_control.msg index 582c9dfb733a..d602158e5d8d 100644 --- a/msg/led_control.msg +++ b/msg/led_control.msg @@ -33,3 +33,5 @@ uint8 mode # see MODE_* uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) + +uint8 ORB_QUEUE_LENGTH = 4 # needs to match BOARD_MAX_LEDS diff --git a/msg/mavlink_log.msg b/msg/mavlink_log.msg index e7387b08ddb1..6a8ab72bc6d1 100644 --- a/msg/mavlink_log.msg +++ b/msg/mavlink_log.msg @@ -2,3 +2,5 @@ uint64 timestamp # time since system start (microseconds) char[50] text uint8 severity # log level (same as in the linux kernel, starting with 0) + +uint8 ORB_QUEUE_LENGTH = 5 diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index 73e9d3bc2279..e3c4963ab24e 100644 --- a/msg/obstacle_distance.msg +++ b/msg/obstacle_distance.msg @@ -1,6 +1,11 @@ # Obstacle distances in front of the sensor. uint64 timestamp # time since system start (microseconds) +uint8 frame #Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is North aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. +uint8 MAV_FRAME_GLOBAL = 0 +uint8 MAV_FRAME_LOCAL_NED = 1 +uint8 MAV_FRAME_BODY_FRD = 12 + uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum. uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index 386493ab9599..e66ce35a6602 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -54,3 +54,5 @@ float32 acceptance_radius # navigation acceptance_radius if we're doing waypoi float32 cruising_speed # the generally desired cruising speed (not a hard constraint) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint) + +bool allow_weather_vane # VTOL: allow (in mission mode) the weather vane feature that turns the nose into the wind diff --git a/msg/qshell_req.msg b/msg/qshell_req.msg index 6ee2a6cb19cf..d472e8dd33b6 100644 --- a/msg/qshell_req.msg +++ b/msg/qshell_req.msg @@ -2,4 +2,4 @@ uint64 timestamp # time since system start (microseconds) char[100] cmd uint32 MAX_STRLEN = 100 uint32 strlen -uint32 sequence +uint32 request_sequence diff --git a/msg/qshell_retval.msg b/msg/qshell_retval.msg index c8c56fd7280e..d42a7715ffe7 100644 --- a/msg/qshell_retval.msg +++ b/msg/qshell_retval.msg @@ -1,3 +1,3 @@ uint64 timestamp # time since system start (microseconds) int32 return_value -uint32 sequence +uint32 return_sequence diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index 01008d54c5ed..145bf25a5c5f 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -1,10 +1,5 @@ uint64 timestamp # time since system start (microseconds) -# rates used by the controller -float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s -float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s -float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s - # rate controller integrator status float32 rollspeed_integ float32 pitchspeed_integ diff --git a/msg/sensor_bias.msg b/msg/sensor_bias.msg index 29a14cb08390..ce81099c99e9 100644 --- a/msg/sensor_bias.msg +++ b/msg/sensor_bias.msg @@ -5,20 +5,10 @@ uint64 timestamp # time since system start (microseconds) -float32 accel_x # Bias corrected acceleration in body X axis (in m/s^2) -float32 accel_y # Bias corrected acceleration in body Y axis (in m/s^2) -float32 accel_z # Bias corrected acceleration in body Z axis (in m/s^2) - # In-run bias estimates (subtract from uncorrected data) -float32 gyro_x_bias # X gyroscope in-run bias in body frame (rad/s, x forward) -float32 gyro_y_bias # Y gyroscope in-run bias in body frame (rad/s, y right) -float32 gyro_z_bias # Z gyroscope in-run bias in body frame (rad/s, z down) +float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) -float32 accel_x_bias # X accelerometer in-run bias in body frame (m/s^2, x forward) -float32 accel_y_bias # Y accelerometer in-run bias in body frame (m/s^2, y right) -float32 accel_z_bias # Z accelerometer in-run bias in body frame (m/s^2, z down) +float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) -float32 mag_x_bias # X magnetometer in-run bias in body frame (Gauss, x forward) -float32 mag_y_bias # Y magnetometer in-run bias in body frame (Gauss, y right) -float32 mag_z_bias # Z magnetometer in-run bias in body frame (Gauss, z down) +float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) diff --git a/msg/sensor_gyro_control.msg b/msg/sensor_gyro_control.msg new file mode 100644 index 000000000000..5151231c4b16 --- /dev/null +++ b/msg/sensor_gyro_control.msg @@ -0,0 +1,11 @@ + +# WARNING: Not recommend for custom development. +# This topic is part of an incremental refactoring of the sensor pipeline and will likely disappear post PX4 release v1.10 + +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 # time the raw data was sampled (microseconds) + +float32[3] xyz # filtered angular velocity in the board axis in rad/s diff --git a/msg/sensor_preflight.msg b/msg/sensor_preflight.msg index c82059537708..1c97183cf7cf 100644 --- a/msg/sensor_preflight.msg +++ b/msg/sensor_preflight.msg @@ -2,7 +2,7 @@ # Pre-flight sensor check metrics. These will be zero if the vehicle only has one sensor. # The topic will not be updated when the vehicle is armed # -uint64 timestamp # time since system start (microseconds) -float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). -float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). -float32 mag_inconsistency_ga # magnitude of maximum difference between Mag instances in (Gauss). +uint64 timestamp # time since system start (microseconds) +float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s). +float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s). +float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 37eaca5e9485..389d5d61839c 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -34,4 +34,4 @@ bool enabled bool ok uint64 subsystem_type -uint32 ORB_QUEUE_LENGTH = 5 +uint8 ORB_QUEUE_LENGTH = 5 diff --git a/msg/task_stack_info.msg b/msg/task_stack_info.msg index c142a9a75207..bb69bf1a8888 100644 --- a/msg/task_stack_info.msg +++ b/msg/task_stack_info.msg @@ -4,3 +4,5 @@ uint64 timestamp # time since system start (microseconds) uint16 stack_free char[24] task_name + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 9d897c644477..3f6f3248c84f 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -55,3 +55,5 @@ float32 rate_rx float32 rate_tx float32 rate_txerr + +uint8 ORB_QUEUE_LENGTH = 3 diff --git a/msg/test_motor.msg b/msg/test_motor.msg index eaad0d74e702..55bfd3f8f29d 100644 --- a/msg/test_motor.msg +++ b/msg/test_motor.msg @@ -3,3 +3,5 @@ uint8 NUM_MOTOR_OUTPUTS = 8 uint32 motor_number # number of motor to spin float32 value # output power, range [0..1] + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py index d7344a7de8d1..24e6c948e9fe 100644 --- a/msg/tools/generate_microRTPS_bridge.py +++ b/msg/tools/generate_microRTPS_bridge.py @@ -62,9 +62,20 @@ def check_rtps_id_uniqueness(classifier): repeated_ids = dict() - full_send_list = dict(list(msg for msg in classifier.msgs_to_send.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_send)) - full_receive_list = dict(list(msg for msg in classifier.msgs_to_receive.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_receive)) - full_ignore_list = dict(list(msg for msg in classifier.msgs_to_ignore.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_ignore)) + if sys.version_info[0] < 3: + full_send_list = dict(list(msg for msg in classifier.msgs_to_send.items( + )) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_send)) + full_receive_list = dict(list(msg for msg in classifier.msgs_to_receive.items( + )) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_receive)) + full_ignore_list = dict(list(msg for msg in classifier.msgs_to_ignore.items( + )) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_ignore)) + else: + full_send_list = dict(list(msg for msg in 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 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 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 full_send_list.items(): @@ -256,15 +267,15 @@ def check_rtps_id_uniqueness(classifier): # uORB templates path uorb_templates_dir = (args.uorb_templates if os.path.isabs(args.uorb_templates) - else os.path.join(msg_dir, args.uorb_templates)) + else os.path.join(msg_dir, args.uorb_templates)) # uRTPS templates path urtps_templates_dir = (args.urtps_templates if os.path.isabs(args.urtps_templates) - else os.path.join(msg_dir, args.urtps_templates)) + else os.path.join(msg_dir, args.urtps_templates)) # parse yaml file into a map of ids 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)) + 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) @@ -298,7 +309,10 @@ def generate_agent(out_dir): if classifier.alias_msgs_to_send: for msg_file in classifier.alias_msgs_to_send: - msg_alias = msg_file[0].keys()[0] + if sys.version_info[0] < 3: + msg_alias = msg_file[0].keys()[0] + else: + msg_alias = list(msg_file[0].keys())[0] msg_name = msg_file[1] if gen_idl: if out_dir != agent_out_dir: @@ -328,7 +342,10 @@ def generate_agent(out_dir): if classifier.alias_msgs_to_receive: for msg_file in classifier.alias_msgs_to_receive: - msg_alias = msg_file[0].keys()[0] + if sys.version_info[0] < 3: + msg_alias = msg_file[0].keys()[0] + else: + msg_alias = list(msg_file[0].keys())[0] msg_name = msg_file[1] if gen_idl: if out_dir != agent_out_dir: @@ -360,11 +377,11 @@ def generate_agent(out_dir): raise Exception("No IDL files found in %s" % idl_dir) if(os.path.exists(fastrtpsgen_path)): for idl_file in glob.glob(os.path.join(idl_dir, "*.idl")): - ret = subprocess.call(fastrtpsgen_path + " -d " + out_dir + - "/fastrtpsgen -example x64Linux2.6gcc " + fastrtpsgen_include + idl_file, shell=True) - if ret: - raise Exception( - "fastrtpsgen failed with code error %s" % ret) + ret = subprocess.call(fastrtpsgen_path + " -d " + out_dir + + "/fastrtpsgen -example x64Linux2.6gcc " + fastrtpsgen_include + idl_file, shell=True) + if ret: + raise Exception( + "fastrtpsgen failed with code error %s" % ret) else: raise Exception( "fastrtpsgen not found. Specify the location of fastrtpsgen with the -f flag") diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py index bab8a202f5c2..4f5b9b6986b8 100755 --- a/msg/tools/px_generate_uorb_topic_files.py +++ b/msg/tools/px_generate_uorb_topic_files.py @@ -178,7 +178,7 @@ def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, pack if (alias != ""): em_globals = get_em_globals( - msg, alias , package, includepath, ids, MsgScope.NONE) + msg, alias, package, includepath, ids, MsgScope.NONE) spec_short_name = alias else: em_globals = get_em_globals( @@ -201,10 +201,24 @@ def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filenam """ Generates source file by msg content """ - send_msgs = list(os.path.join(msg_dir, msg + ".msg") for msg in filename_send_msgs) - alias_send_msgs = list([os.path.join(msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_send_msgs) - receive_msgs = list(os.path.join(msg_dir, msg + ".msg") for msg in filename_receive_msgs) - alias_receive_msgs = list([os.path.join(msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_receive_msgs) + send_msgs = list(os.path.join(msg_dir, msg + ".msg") + for msg in filename_send_msgs) + receive_msgs = list(os.path.join(msg_dir, msg + ".msg") + for msg in filename_receive_msgs) + + if sys.version_info[0] < 3: + alias_send_msgs = list([os.path.join( + msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_send_msgs) + else: + alias_send_msgs = list([os.path.join(msg_dir, msg[1] + ".msg"), + list(msg[0].keys())[0]] for msg in filename_alias_send_msgs) + + if sys.version_info[0] < 3: + alias_receive_msgs = list([os.path.join( + msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_receive_msgs) + else: + alias_receive_msgs = list([os.path.join( + msg_dir, msg[1] + ".msg"), list(msg[0].keys())[0]] for msg in filename_alias_receive_msgs) em_globals_list = [] if send_msgs: diff --git a/msg/tools/uorb_rtps_classifier.py b/msg/tools/uorb_rtps_classifier.py index f2cfd6b7d15a..594bdb018dce 100644 --- a/msg/tools/uorb_rtps_classifier.py +++ b/msg/tools/uorb_rtps_classifier.py @@ -49,10 +49,12 @@ def __init__(self, yaml_file, msg_folder): 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 = 150 # Checkers self.check_if_listed(yaml_file) 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() @@ -124,7 +126,7 @@ def check_if_listed(self, yaml_file): none_listed_msgs = [] for msg in self.all_msgs_list: result = not any( - dict.values()[0] == msg for dict in self.msg_id_map['rtps']) + dict['msg'] == msg for dict in self.msg_id_map['rtps']) if result: none_listed_msgs.append(msg) @@ -136,7 +138,7 @@ def check_if_listed(self, yaml_file): 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" + + "\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 has multi-topics (#TOPICS), these should be added as well.\n") @@ -144,19 +146,45 @@ def check_base_type(self): """ Check if alias message has correct base type """ - rtps_registered_msgs = list( + registered_alias_msgs = list( dict['alias'] for dict in self.msg_id_map['rtps'] if 'alias' in dict.keys()) - uorb_msg = list(msg for msg in self.all_msgs_list) - incorrect_base_types = list(set(rtps_registered_msgs) - set(uorb_msg)) - base_types = {} + base_types = [] + for dict in self.msg_id_map['rtps']: + if 'alias' not in dict.keys(): + base_types.append(dict['msg']) + + incorrect_base_types = list( + set(registered_alias_msgs) - set(base_types)) + + base_types_suggestion = {} for incorrect in incorrect_base_types: - base_types.update({incorrect: difflib.get_close_matches( - incorrect, uorb_msg, n=1, cutoff=0.6)}) + base_types_suggestion.update({incorrect: difflib.get_close_matches( + incorrect, base_types, n=1, cutoff=0.6)}) + + if len(base_types_suggestion) > 0: + 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 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 dict.keys() and dict['id'] >= self.alias_space_init_id: + incorrect_base_ids.update({dict['msg']: dict['id']}) + elif 'alias' in dict.keys() and dict['id'] < self.alias_space_init_id: + incorrect_alias_ids.update({dict['msg']: dict['id']}) - if len(base_types) > 0: + if len(incorrect_base_ids) > 0: raise AssertionError( - ('\n' + '\n'.join('\t- The multi-topic message base type {} does not exist. Did you mean \'{}\'?'.format(k, v[0]) for k, v in base_types.items()))) + ('\n' + '\n'.join('\t- The message \'{} with ID \'{}\' is in the wrong ID space. Please use any of the available IDs from 0 to 149'.format(k, v) for k, v in 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 149 to 255'.format(k, v) for k, v in incorrect_alias_ids.items()))) @staticmethod def parse_yaml_msg_id_file(yaml_file): @@ -210,9 +238,14 @@ def parse_yaml_msg_id_file(yaml_file): for msg_file in classifier.msgs_files_send) + '\n') else: if args.alias: - print (', '.join(str(msg) - for msg in classifier.msgs_to_send.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0]) - for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n') + if sys.version_info[0] < 3: + print (', '.join(str(msg) + for msg in classifier.msgs_to_send.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[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 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') else: print (', '.join(str(msg) for msg in classifier.msgs_to_send.keys())) @@ -222,9 +255,14 @@ def parse_yaml_msg_id_file(yaml_file): for msg_file in classifier.msgs_files_receive) + '\n') else: if args.alias: - print (', '.join(str(msg) - for msg in classifier.msgs_to_receive.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0]) - for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n') + if sys.version_info[0] < 3: + print (', '.join(str(msg) + for msg in classifier.msgs_to_receive.keys()) + (' alias ' + ', '.join(str(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 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 classifier.msgs_to_receive.keys())) @@ -234,9 +272,14 @@ def parse_yaml_msg_id_file(yaml_file): for msg_file in classifier.msgs_files_ignore) + '\n') else: if args.alias: - print (', '.join(str(msg) - for msg in classifier.msgs_to_ignore.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0]) - for msg in classifier.alias_msgs_to_ignore) if len(classifier.alias_msgs_to_ignore) > 0 else '') + '\n') + if sys.version_info[0] < 3: + print (', '.join(str(msg) + for msg in classifier.msgs_to_ignore.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0]) + for msg in classifier.alias_msgs_to_ignore) if len(classifier.alias_msgs_to_ignore) > 0 else '') + '\n') + else: + print (', '.join(str(msg) + for msg in 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') else: print (', '.join(str(msg) for msg in classifier.msgs_to_ignore.keys())) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 2237f6d31c6e..f84cc0dcba6a 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -252,48 +252,74 @@ rtps: id: 109 - msg: landing_gear id: 110 - # multi topics + - msg: wheel_encoders + id: 111 + - msg: vehicle_angular_velocity + id: 112 + - msg: vehicle_acceleration + id: 113 + - msg: sensor_gyro_control + id: 114 + - msg: airspeed_validated + id: 115 + ########## multi topics: begin ########## - msg: actuator_controls_0 - id: 120 + id: 150 alias: actuator_controls - msg: actuator_controls_1 - id: 121 + id: 151 alias: actuator_controls - msg: actuator_controls_2 - id: 122 + id: 152 alias: actuator_controls - msg: actuator_controls_3 - id: 123 + id: 153 alias: actuator_controls - msg: actuator_controls_virtual_fw - id: 124 + id: 154 alias: actuator_controls - msg: actuator_controls_virtual_mc - id: 125 + id: 155 alias: actuator_controls - msg: mc_virtual_attitude_setpoint - id: 126 + id: 156 alias: vehicle_attitude_setpoint - msg: fw_virtual_attitude_setpoint - id: 127 + id: 157 alias: vehicle_attitude_setpoint - msg: vehicle_attitude_groundtruth - id: 128 + id: 158 alias: vehicle_attitude - msg: vehicle_global_position_groundtruth - id: 129 + id: 159 alias: vehicle_global_position - msg: vehicle_local_position_groundtruth - id: 130 + id: 160 alias: vehicle_local_position - msg: vehicle_mocap_odometry alias: vehicle_odometry - id: 131 + id: 161 receive: true - msg: vehicle_visual_odometry - id: 132 + id: 162 alias: vehicle_odometry receive: true - msg: vehicle_trajectory_waypoint_desired - id: 133 + id: 163 alias: vehicle_trajectory_waypoint + - msg: obstacle_distance_fused + id: 164 + alias: obstacle_distance + - msg: vehicle_vision_attitude + id: 165 + alias: vehicle_attitude + - msg: trajectory_setpoint + id: 166 + alias: vehicle_local_position_setpoint + - msg: camera_trigger_secondary + id: 167 + alias: camera_trigger + - msg: vehicle_angular_velocity_groundtruth + id: 168 + alias: vehicle_angular_velocity + ########## multi topics: end ########## diff --git a/msg/trajectory_waypoint.msg b/msg/trajectory_waypoint.msg index 22d222423a30..6ea9bae4d34e 100644 --- a/msg/trajectory_waypoint.msg +++ b/msg/trajectory_waypoint.msg @@ -10,3 +10,4 @@ float32 yaw float32 yaw_speed bool point_valid +uint8 type diff --git a/msg/transponder_report.msg b/msg/transponder_report.msg index 603f034fa42a..99d2a332b341 100644 --- a/msg/transponder_report.msg +++ b/msg/transponder_report.msg @@ -22,4 +22,4 @@ uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16 uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32 uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256 -uint32 ORB_QUEUE_LENGTH = 10 +uint8 ORB_QUEUE_LENGTH = 10 diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg index e6e480ebd407..cb188ce2974b 100644 --- a/msg/uavcan_parameter_request.msg +++ b/msg/uavcan_parameter_request.msg @@ -7,3 +7,5 @@ int16 param_index # -1 if the param_id field should be used as identifier uint8 param_type # MAVLink parameter type int64 int_value # current value if param_type is int-like float32 real_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 3 diff --git a/msg/ulog_stream.msg b/msg/ulog_stream.msg index e88de59f0dfd..7eaa32778dd1 100644 --- a/msg/ulog_stream.msg +++ b/msg/ulog_stream.msg @@ -15,3 +15,5 @@ uint8 first_message_offset # offset into data where first message starts. This uint16 sequence # allows determine drops uint8 flags # see FLAGS_* uint8[249] data # ulog data + +uint8 ORB_QUEUE_LENGTH = 14 # TODO: we might be able to reduce this if mavlink polled on the topic diff --git a/msg/vehicle_acceleration.msg b/msg/vehicle_acceleration.msg new file mode 100644 index 000000000000..9f6e8664727d --- /dev/null +++ b/msg/vehicle_acceleration.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected acceleration (including gravity) in body axis (in m/s^2) diff --git a/msg/vehicle_angular_velocity.msg b/msg/vehicle_angular_velocity.msg new file mode 100644 index 000000000000..3f1b0a7720d5 --- /dev/null +++ b/msg/vehicle_angular_velocity.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected angular velocity about X, Y, Z body axis in rad/s + +# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index ae42454c71c1..874e73804fee 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -2,11 +2,7 @@ uint64 timestamp # time since system start (microseconds) -float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s -float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s -float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s - -float32[4] q # Quaternion rotation from NED earth frame to XYZ body frame +float32[4] q # Quaternion rotation from XYZ body frame to NED earth frame. float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg index 0f7d29177497..4dedf1879d9a 100644 --- a/msg/vehicle_command_ack.msg +++ b/msg/vehicle_command_ack.msg @@ -13,7 +13,7 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 -uint32 ORB_QUEUE_LENGTH = 3 +uint8 ORB_QUEUE_LENGTH = 3 uint16 command uint8 result diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 5a61f7296e6d..1d5351c2f65c 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -6,7 +6,7 @@ uint8 ARMING_STATE_INIT = 0 uint8 ARMING_STATE_STANDBY = 1 uint8 ARMING_STATE_ARMED = 2 uint8 ARMING_STATE_STANDBY_ERROR = 3 -uint8 ARMING_STATE_REBOOT = 4 +uint8 ARMING_STATE_SHUTDOWN = 4 uint8 ARMING_STATE_IN_AIR_RESTORE = 5 uint8 ARMING_STATE_MAX = 6 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 923f62c56959..a5dd3dec1ed7 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -14,7 +14,7 @@ bool condition_local_velocity_valid # set to true by the commander app if the q bool condition_local_altitude_valid 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 circuit_breaker_engaged_power_check bool circuit_breaker_engaged_airspd_check bool circuit_breaker_engaged_enginefailure_check diff --git a/msg/wheel_encoders.msg b/msg/wheel_encoders.msg new file mode 100644 index 000000000000..1654902cdf00 --- /dev/null +++ b/msg/wheel_encoders.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int64 encoder_position # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation +int32 speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse +uint32 pulses_per_rev # Number of pulses per revolution for each wheel diff --git a/src/platforms/CMakeLists.txt b/platforms/CMakeLists.txt similarity index 99% rename from src/platforms/CMakeLists.txt rename to platforms/CMakeLists.txt index a2a39344fc75..cca36ff6c781 100644 --- a/src/platforms/CMakeLists.txt +++ b/platforms/CMakeLists.txt @@ -32,3 +32,5 @@ ############################################################################ add_subdirectory(common) + + diff --git a/src/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt similarity index 100% rename from src/platforms/common/CMakeLists.txt rename to platforms/common/CMakeLists.txt diff --git a/src/platforms/apps.cpp.in b/platforms/common/apps.cpp.in similarity index 100% rename from src/platforms/apps.cpp.in rename to platforms/common/apps.cpp.in diff --git a/src/platforms/apps.h.in b/platforms/common/apps.h.in similarity index 95% rename from src/platforms/apps.h.in rename to platforms/common/apps.h.in index c3cc46cd446a..37728e0fd92f 100644 --- a/src/platforms/apps.h.in +++ b/platforms/common/apps.h.in @@ -4,6 +4,7 @@ #include "px4_tasks.h" // px4_main_t #include +#include // Maps an app name to it's function. typedef std::map apps_map_type; diff --git a/src/platforms/empty.c b/platforms/common/empty.c similarity index 100% rename from src/platforms/empty.c rename to platforms/common/empty.c diff --git a/src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp similarity index 100% rename from src/platforms/common/px4_work_queue/ScheduledWorkItem.hpp rename to platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp diff --git a/src/platforms/common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp similarity index 79% rename from src/platforms/common/px4_work_queue/WorkItem.hpp rename to platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 2135d0367c62..332982549990 100644 --- a/src/platforms/common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -57,8 +57,25 @@ class WorkItem : public IntrusiveQueueNode virtual void Run() = 0; + /** + * Switch to a different WorkQueue. + * NOTE: Caller is responsible for synchronization. + * + * @param config The WorkQueue configuration (see WorkQueueManager.hpp). + * @return true if initialization was successful + */ + bool ChangeWorkQeue(const wq_config_t &config) { return Init(config); } + protected: + /** + * Initialize WorkItem given a WorkQueue config. This call + * can also be used to switch to a different WorkQueue. + * NOTE: Caller is responsible for synchronization. + * + * @param config The WorkQueue configuration (see WorkQueueManager.hpp). + * @return true if initialization was successful + */ bool Init(const wq_config_t &config); void Deinit(); diff --git a/src/platforms/common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp similarity index 100% rename from src/platforms/common/px4_work_queue/WorkQueue.hpp rename to platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp similarity index 87% rename from src/platforms/common/px4_work_queue/WorkQueueManager.hpp rename to platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index d53813f8eeb4..2f0cec8b06e8 100644 --- a/src/platforms/common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,22 +50,22 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI1{"wq:SPI1", 1250, -1}; -static constexpr wq_config_t SPI2{"wq:SPI2", 1250, -2}; -static constexpr wq_config_t SPI3{"wq:SPI3", 1250, -3}; -static constexpr wq_config_t SPI4{"wq:SPI4", 1250, -4}; -static constexpr wq_config_t SPI5{"wq:SPI5", 1250, -5}; -static constexpr wq_config_t SPI6{"wq:SPI6", 1250, -6}; +static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1}; +static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2}; +static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3}; +static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4}; +static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5}; +static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6}; static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7}; static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8}; static constexpr wq_config_t I2C3{"wq:I2C3", 1250, -9}; static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10}; -static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 7000, -11}; // PX4 att/pos controllers, highest priority after sensors +static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 2000, -11}; // PX4 att/pos controllers, highest priority after sensors static constexpr wq_config_t hp_default{"wq:hp_default", 1500, -12}; -static constexpr wq_config_t lp_default{"wq:lp_default", 1500, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 1700, -50}; static constexpr wq_config_t test1{"wq:test1", 800, 0}; static constexpr wq_config_t test2{"wq:test2", 800, 0}; diff --git a/src/platforms/common/module.cpp b/platforms/common/module.cpp similarity index 100% rename from src/platforms/common/module.cpp rename to platforms/common/module.cpp diff --git a/src/platforms/common/px4_cli.cpp b/platforms/common/px4_cli.cpp similarity index 100% rename from src/platforms/common/px4_cli.cpp rename to platforms/common/px4_cli.cpp diff --git a/src/platforms/common/px4_getopt.c b/platforms/common/px4_getopt.c similarity index 100% rename from src/platforms/common/px4_getopt.c rename to platforms/common/px4_getopt.c diff --git a/src/platforms/common/px4_log.c b/platforms/common/px4_log.c similarity index 100% rename from src/platforms/common/px4_log.c rename to platforms/common/px4_log.c diff --git a/src/platforms/common/px4_work_queue/CMakeLists.txt b/platforms/common/px4_work_queue/CMakeLists.txt similarity index 100% rename from src/platforms/common/px4_work_queue/CMakeLists.txt rename to platforms/common/px4_work_queue/CMakeLists.txt diff --git a/src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp similarity index 97% rename from src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp rename to platforms/common/px4_work_queue/ScheduledWorkItem.cpp index d729e241340f..72c45598a77d 100644 --- a/src/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include "ScheduledWorkItem.hpp" +#include namespace px4 { diff --git a/src/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp similarity index 92% rename from src/platforms/common/px4_work_queue/WorkItem.cpp rename to platforms/common/px4_work_queue/WorkItem.cpp index cbf8541b20fb..865df26e50af 100644 --- a/src/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -31,10 +31,10 @@ * ****************************************************************************/ -#include "WorkItem.hpp" +#include -#include "WorkQueue.hpp" -#include "WorkQueueManager.hpp" +#include +#include #include #include diff --git a/src/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp similarity index 96% rename from src/platforms/common/px4_work_queue/WorkQueue.cpp rename to platforms/common/px4_work_queue/WorkQueue.cpp index e9fd51be4d42..41641bae180b 100644 --- a/src/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -31,8 +31,8 @@ * ****************************************************************************/ -#include "WorkQueue.hpp" -#include "WorkItem.hpp" +#include +#include #include diff --git a/src/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp similarity index 97% rename from src/platforms/common/px4_work_queue/WorkQueueManager.cpp rename to platforms/common/px4_work_queue/WorkQueueManager.cpp index f9d092396f60..471299a55d4f 100644 --- a/src/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -31,9 +31,9 @@ * ****************************************************************************/ -#include "WorkQueueManager.hpp" +#include -#include "WorkQueue.hpp" +#include #include #include @@ -171,7 +171,7 @@ static void *WorkQueueRunner(void *context) return nullptr; } -static void WorkQueueManagerRun() +static int WorkQueueManagerRun(int, char **) { _wq_manager_wqs_list = new BlockingList(); _wq_manager_create_queue = new BlockingQueue(); @@ -246,6 +246,8 @@ static void WorkQueueManagerRun() } } } + + return 0; } int WorkQueueManagerStart() diff --git a/src/platforms/common/px4_work_queue/test/CMakeLists.txt b/platforms/common/px4_work_queue/test/CMakeLists.txt similarity index 100% rename from src/platforms/common/px4_work_queue/test/CMakeLists.txt rename to platforms/common/px4_work_queue/test/CMakeLists.txt diff --git a/src/platforms/common/px4_work_queue/test/wqueue_main.cpp b/platforms/common/px4_work_queue/test/wqueue_main.cpp similarity index 98% rename from src/platforms/common/px4_work_queue/test/wqueue_main.cpp rename to platforms/common/px4_work_queue/test/wqueue_main.cpp index aeb95c622b8b..e2106b9294de 100644 --- a/src/platforms/common/px4_work_queue/test/wqueue_main.cpp +++ b/platforms/common/px4_work_queue/test/wqueue_main.cpp @@ -35,8 +35,8 @@ #include "wqueue_scheduled_test.h" #include -#include #include +#include #include int PX4_MAIN(int argc, char **argv) diff --git a/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp b/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp similarity index 100% rename from src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp rename to platforms/common/px4_work_queue/test/wqueue_scheduled_test.cpp diff --git a/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h b/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h similarity index 96% rename from src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h rename to platforms/common/px4_work_queue/test/wqueue_scheduled_test.h index 538267003325..52ba1c98b25b 100644 --- a/src/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h +++ b/platforms/common/px4_work_queue/test/wqueue_scheduled_test.h @@ -34,7 +34,7 @@ #pragma once #include -#include +#include #include using namespace px4; diff --git a/src/platforms/common/px4_work_queue/test/wqueue_start.cpp b/platforms/common/px4_work_queue/test/wqueue_start.cpp similarity index 100% rename from src/platforms/common/px4_work_queue/test/wqueue_start.cpp rename to platforms/common/px4_work_queue/test/wqueue_start.cpp diff --git a/src/platforms/common/px4_work_queue/test/wqueue_test.cpp b/platforms/common/px4_work_queue/test/wqueue_test.cpp similarity index 100% rename from src/platforms/common/px4_work_queue/test/wqueue_test.cpp rename to platforms/common/px4_work_queue/test/wqueue_test.cpp diff --git a/src/platforms/common/px4_work_queue/test/wqueue_test.h b/platforms/common/px4_work_queue/test/wqueue_test.h similarity index 97% rename from src/platforms/common/px4_work_queue/test/wqueue_test.h rename to platforms/common/px4_work_queue/test/wqueue_test.h index 4935901b7435..573fea900749 100644 --- a/src/platforms/common/px4_work_queue/test/wqueue_test.h +++ b/platforms/common/px4_work_queue/test/wqueue_test.h @@ -34,7 +34,7 @@ #pragma once #include -#include +#include #include using namespace px4; diff --git a/src/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp similarity index 100% rename from src/platforms/common/shutdown.cpp rename to platforms/common/shutdown.cpp diff --git a/src/platforms/common/work_queue/CMakeLists.txt b/platforms/common/work_queue/CMakeLists.txt similarity index 100% rename from src/platforms/common/work_queue/CMakeLists.txt rename to platforms/common/work_queue/CMakeLists.txt diff --git a/src/platforms/common/work_queue/dq_addlast.c b/platforms/common/work_queue/dq_addlast.c similarity index 100% rename from src/platforms/common/work_queue/dq_addlast.c rename to platforms/common/work_queue/dq_addlast.c diff --git a/src/platforms/common/work_queue/dq_rem.c b/platforms/common/work_queue/dq_rem.c similarity index 100% rename from src/platforms/common/work_queue/dq_rem.c rename to platforms/common/work_queue/dq_rem.c diff --git a/src/platforms/common/work_queue/dq_remfirst.c b/platforms/common/work_queue/dq_remfirst.c similarity index 100% rename from src/platforms/common/work_queue/dq_remfirst.c rename to platforms/common/work_queue/dq_remfirst.c diff --git a/src/platforms/common/work_queue/hrt_queue.c b/platforms/common/work_queue/hrt_queue.c similarity index 100% rename from src/platforms/common/work_queue/hrt_queue.c rename to platforms/common/work_queue/hrt_queue.c diff --git a/src/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c similarity index 100% rename from src/platforms/common/work_queue/hrt_thread.c rename to platforms/common/work_queue/hrt_thread.c diff --git a/src/platforms/common/work_queue/hrt_work_cancel.c b/platforms/common/work_queue/hrt_work_cancel.c similarity index 100% rename from src/platforms/common/work_queue/hrt_work_cancel.c rename to platforms/common/work_queue/hrt_work_cancel.c diff --git a/src/platforms/common/work_queue/queue.c b/platforms/common/work_queue/queue.c similarity index 100% rename from src/platforms/common/work_queue/queue.c rename to platforms/common/work_queue/queue.c diff --git a/src/platforms/common/work_queue/sq_addafter.c b/platforms/common/work_queue/sq_addafter.c similarity index 100% rename from src/platforms/common/work_queue/sq_addafter.c rename to platforms/common/work_queue/sq_addafter.c diff --git a/src/platforms/common/work_queue/sq_addlast.c b/platforms/common/work_queue/sq_addlast.c similarity index 100% rename from src/platforms/common/work_queue/sq_addlast.c rename to platforms/common/work_queue/sq_addlast.c diff --git a/src/platforms/common/work_queue/sq_remfirst.c b/platforms/common/work_queue/sq_remfirst.c similarity index 100% rename from src/platforms/common/work_queue/sq_remfirst.c rename to platforms/common/work_queue/sq_remfirst.c diff --git a/src/platforms/common/work_queue/work_cancel.c b/platforms/common/work_queue/work_cancel.c similarity index 100% rename from src/platforms/common/work_queue/work_cancel.c rename to platforms/common/work_queue/work_cancel.c diff --git a/src/platforms/common/work_queue/work_lock.c b/platforms/common/work_queue/work_lock.c similarity index 100% rename from src/platforms/common/work_queue/work_lock.c rename to platforms/common/work_queue/work_lock.c diff --git a/src/platforms/common/work_queue/work_lock.h b/platforms/common/work_queue/work_lock.h similarity index 100% rename from src/platforms/common/work_queue/work_lock.h rename to platforms/common/work_queue/work_lock.h diff --git a/src/platforms/common/work_queue/work_queue.c b/platforms/common/work_queue/work_queue.c similarity index 100% rename from src/platforms/common/work_queue/work_queue.c rename to platforms/common/work_queue/work_queue.c diff --git a/src/platforms/common/work_queue/work_thread.c b/platforms/common/work_queue/work_thread.c similarity index 100% rename from src/platforms/common/work_queue/work_thread.c rename to platforms/common/work_queue/work_thread.c diff --git a/src/platforms/common/work_queue/wqueue_test/CMakeLists.txt b/platforms/common/work_queue/wqueue_test/CMakeLists.txt similarity index 100% rename from src/platforms/common/work_queue/wqueue_test/CMakeLists.txt rename to platforms/common/work_queue/wqueue_test/CMakeLists.txt diff --git a/src/platforms/common/work_queue/wqueue_test/wqueue_main.cpp b/platforms/common/work_queue/wqueue_test/wqueue_main.cpp similarity index 98% rename from src/platforms/common/work_queue/wqueue_test/wqueue_main.cpp rename to platforms/common/work_queue/wqueue_test/wqueue_main.cpp index 9f693c62a922..fb222df24c68 100644 --- a/src/platforms/common/work_queue/wqueue_test/wqueue_main.cpp +++ b/platforms/common/work_queue/wqueue_test/wqueue_main.cpp @@ -40,7 +40,6 @@ #include "wqueue_test.h" #include -#include #include #include diff --git a/src/platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp b/platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp similarity index 100% rename from src/platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp rename to platforms/common/work_queue/wqueue_test/wqueue_start_posix.cpp diff --git a/src/platforms/common/work_queue/wqueue_test/wqueue_test.cpp b/platforms/common/work_queue/wqueue_test/wqueue_test.cpp similarity index 100% rename from src/platforms/common/work_queue/wqueue_test/wqueue_test.cpp rename to platforms/common/work_queue/wqueue_test/wqueue_test.cpp diff --git a/src/platforms/common/work_queue/wqueue_test/wqueue_test.h b/platforms/common/work_queue/wqueue_test/wqueue_test.h similarity index 100% rename from src/platforms/common/work_queue/wqueue_test/wqueue_test.h rename to platforms/common/work_queue/wqueue_test/wqueue_test.h diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 406e6432ff65..afc3c6c8b283 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -36,9 +36,7 @@ include(cygwin_cygpath) set(NUTTX_DIR ${PX4_BINARY_DIR}/NuttX/nuttx) set(NUTTX_APPS_DIR ${PX4_BINARY_DIR}/NuttX/apps) -add_subdirectory(src) - -add_executable(px4 ${PX4_SOURCE_DIR}/src/platforms/empty.c) +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) @@ -73,12 +71,16 @@ file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_D # because even relative linker script paths are different for linux, mac and windows CYGPATH(PX4_BINARY_DIR PX4_BINARY_DIR_CYG) +if(POLICY CMP0079) + cmake_policy(SET CMP0079 NEW) +endif() + target_link_libraries(nuttx_arch INTERFACE drivers_board - drivers_arch drivers_boards_common drivers_boards_common_arch + arch_hrt ) target_link_libraries(nuttx_c INTERFACE nuttx_drivers) @@ -172,7 +174,8 @@ if (TARGET parameters_xml AND TARGET airframes_xml) /dev/serial/by-id/usb-Bitcraze* /dev/serial/by-id/pci-Bitcraze* /dev/serial/by-id/usb-Gumstix* - + /dev/serial/by-id/usb-Hex_ProfiCNC* + /dev/serial/by-id/usb-UVify_FMU_BL* ) elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin") @@ -245,48 +248,6 @@ add_custom_target(stack_check VERBATIM ) -find_program(BLOATY_PROGRAM bloaty) -if (BLOATY_PROGRAM) - # bloaty symbols - add_custom_target(bloaty_symbols - COMMAND ${BLOATY_PROGRAM} -d symbols -C full -n 50 -s vm $ - DEPENDS px4 - USES_TERMINAL - ) - - # bloaty compilation units - add_custom_target(bloaty_compileunits - COMMAND ${BLOATY_PROGRAM} -d compileunits -C full -n 50 -s vm $ - DEPENDS px4 - USES_TERMINAL - ) - - # bloaty templates - add_custom_target(bloaty_templates - COMMAND ${BLOATY_PROGRAM} -d shortsymbols,fullsymbols -n 50 $ - DEPENDS px4 - USES_TERMINAL - ) - - # bloaty inlines - add_custom_target(bloaty_inlines - COMMAND ${BLOATY_PROGRAM} -d inlines -C full -n 50 $ - DEPENDS px4 - USES_TERMINAL - ) - - # bloaty compare with last master build - add_custom_target(bloaty_compare_master - #COMMAND wget --no-verbose https://s3.amazonaws.com/px4-travis/Firmware/master/${FW_NAME} -O master_${FW_NAME} - COMMAND wget --no-verbose https://s3.amazonaws.com/px4-travis/Firmware/master/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf -O master_${FW_NAME} - COMMAND ${BLOATY_PROGRAM} -d symbols -n 50 -C full -s vm $ -- master_${FW_NAME} - DEPENDS px4 - WORKING_DIRECTORY ${PX4_BINARY_DIR} - VERBATIM - USES_TERMINAL - ) -endif() - # debugger helpers configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.gdbinit) diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 257e1730dca1..d8da51108264 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 257e1730dca1faafdf3a694ba53da423f46d569c +Subproject commit d8da511082646d83a54c6905daca13f0a1a609f0 diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 3c9b27ef58e0..7c98a88b4d24 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -38,6 +38,7 @@ # Required OS Interface Functions # # * px4_os_add_flags +# * px4_os_determine_build_chip # * px4_os_prebuild_targets # @@ -84,6 +85,41 @@ function(px4_os_add_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) + + # determine chip and chip manufacturer based on NuttX config + if (CONFIG_STM32_STM32F10XX) + set(CHIP_MANUFACTURER "stm") + set(CHIP "stm32f1") + elseif(CONFIG_STM32_STM32F30XX) + set(CHIP_MANUFACTURER "stm") + set(CHIP "stm32f3") + elseif(CONFIG_STM32_STM32F4XXX) + set(CHIP_MANUFACTURER "stm") + set(CHIP "stm32f4") + elseif(CONFIG_ARCH_CHIP_STM32F7) + set(CHIP_MANUFACTURER "stm") + set(CHIP "stm32f7") + elseif(CONFIG_ARCH_CHIP_MK66FN2M0VMD18) + set(CHIP_MANUFACTURER "nxp") + set(CHIP "k66") + else() + message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.") + endif() + + set(PX4_CHIP ${CHIP} CACHE STRING "PX4 Chip" FORCE) + set(PX4_CHIP_MANUFACTURER ${CHIP_MANUFACTURER} CACHE STRING "PX4 Chip Manufacturer" FORCE) +endfunction() + #============================================================================= # # px4_os_prebuild_targets diff --git a/src/drivers/stm32/adc/CMakeLists.txt b/platforms/nuttx/src/px4/CMakeLists.txt similarity index 92% rename from src/drivers/stm32/adc/CMakeLists.txt rename to platforms/nuttx/src/px4/CMakeLists.txt index 46f8f429de00..929788b587e2 100644 --- a/src/drivers/stm32/adc/CMakeLists.txt +++ b/platforms/nuttx/src/px4/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,7 @@ # ############################################################################ -px4_add_module( - MODULE drivers__adc - MAIN adc - SRCS - adc.cpp - ) +add_subdirectory(common) + +add_subdirectory(${PX4_CHIP_MANUFACTURER}) + diff --git a/platforms/nuttx/src/px4_layer/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt similarity index 96% rename from platforms/nuttx/src/px4_layer/CMakeLists.txt rename to platforms/nuttx/src/px4/common/CMakeLists.txt index 8d5e50dbaed4..967dd6e46785 100644 --- a/platforms/nuttx/src/px4_layer/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -48,6 +48,6 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io") px4_work_queue ) else() - add_library(px4_layer ${PX4_SOURCE_DIR}/src/platforms/empty.c) + add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c) endif() add_dependencies(px4_layer prebuild_targets) diff --git a/platforms/nuttx/src/px4_layer/console_buffer.cpp b/platforms/nuttx/src/px4/common/console_buffer.cpp similarity index 100% rename from platforms/nuttx/src/px4_layer/console_buffer.cpp rename to platforms/nuttx/src/px4/common/console_buffer.cpp diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/adc.h b/platforms/nuttx/src/px4/common/include/px4_platform/adc.h new file mode 100644 index 000000000000..d7f6e85a5b94 --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/adc.h @@ -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. + * + ****************************************************************************/ +#pragma once + +#if SYSTEM_ADC_BASE == HW_REV_VER_ADC_BASE +# define SYSTEM_ADC_COUNT 1 +#else +# define SYSTEM_ADC_COUNT 2 +#endif + diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h new file mode 100644 index 000000000000..8bf62a5f8dbd --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 + + +__BEGIN_DECLS +#include +#include + +/* For historical reasons (NuttX STM32 numbering) PX4 bus numbering is 1 based + * All PX4 code, including, board code is written to assuming 1 based numbering. + * The following macros are used to allow the board config to define the bus + * numbers in terms of the NuttX driver numbering. 1,2,3 for one based numbering + * schemes or 0,1,2 for zero based schemes. + */ + +#define PX4_BUS_NUMBER_TO_PX4(x) ((x)+PX4_BUS_OFFSET) /* Use to define Zero based to match Nuttx Driver but provide 1 based to PX4 */ +#define PX4_BUS_NUMBER_FROM_PX4(x) ((x)-PX4_BUS_OFFSET) /* Use to map PX4 1 based to NuttX driver 0 based */ + +#define px4_enter_critical_section() enter_critical_section() +#define px4_leave_critical_section(flags) leave_critical_section(flags) + +#include + +__END_DECLS + diff --git a/platforms/nuttx/src/px4_layer/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp similarity index 98% rename from platforms/nuttx/src/px4_layer/px4_init.cpp rename to platforms/nuttx/src/px4/common/px4_init.cpp index 466fcc4afd85..7748f7b94c6a 100644 --- a/platforms/nuttx/src/px4_layer/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include diff --git a/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp b/platforms/nuttx/src/px4/common/px4_nuttx_impl.cpp similarity index 95% rename from platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp rename to platforms/nuttx/src/px4/common/px4_nuttx_impl.cpp index e760cee5e9b9..30780b11d70f 100644 --- a/platforms/nuttx/src/px4_layer/px4_nuttx_impl.cpp +++ b/platforms/nuttx/src/px4/common/px4_nuttx_impl.cpp @@ -38,7 +38,6 @@ */ #include -#include #include namespace px4 @@ -49,9 +48,4 @@ void init(int argc, char *argv[], const char *process_name) printf("process: %s\n", process_name); } -uint64_t get_time_micros() -{ - return hrt_absolute_time(); -} - } diff --git a/platforms/nuttx/src/px4_layer/px4_nuttx_tasks.c b/platforms/nuttx/src/px4/common/px4_nuttx_tasks.c similarity index 100% rename from platforms/nuttx/src/px4_layer/px4_nuttx_tasks.c rename to platforms/nuttx/src/px4/common/px4_nuttx_tasks.c diff --git a/platforms/nuttx/src/px4/nxp/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/CMakeLists.txt new file mode 100644 index 000000000000..ec489bf559c3 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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/nuttx/src/px4/nxp/k66/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt new file mode 100644 index 000000000000..7f95fdb519c8 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(../kinetis/adc adc) +add_subdirectory(../kinetis/led_pwm led_pwm) +add_subdirectory(../kinetis/hrt hrt) +add_subdirectory(../kinetis/io_pins io_pins) +add_subdirectory(../kinetis/tone_alarm tone_alarm) + + + diff --git a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h new file mode 100644 index 000000000000..743deaed331c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../kinetis/include/px4_arch/adc.h" + diff --git a/src/drivers/kinetis/drv_pwm_servo.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h similarity index 90% rename from src/drivers/kinetis/drv_pwm_servo.h rename to platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h index e3477183d2ab..11661f853a71 100644 --- a/src/drivers/kinetis/drv_pwm_servo.h +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,13 +30,8 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once -/** - * @file drv_pwm_servo.h - * - * stm32-specific PWM output data. - */ -#pragma once +#include "../../../kinetis/include/px4_arch/io_timer.h" -#include 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 new file mode 100644 index 000000000000..33930f5235db --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 + +__BEGIN_DECLS + +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_KINETISK66 + +// Fixme: using ?? +#define PX4_BBSRAM_SIZE 2048 +#define PX4_BBSRAM_GETDESC_IOCTL 0 +#define PX4_NUMBER_I2C_BUSES KINETIS_NI2C + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +#include + +/* Kinetis defines the 128 bit UUID as + * init32_t[4] that can be read as words + * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 127:96 (offset 0) + * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 95:64 (offset 4) + * init32_t[2] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 8) + * init32_t[3] PX4_CPU_UUID_ADDRESS[3] bits 31:0 (offset C) + * + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] [2] [3] + * bits 127:96 95-64 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH KINETIS_UID_SIZE +#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 + +/* define common formating across all commands */ + +#define PX4_CPU_UUID_WORD32_FORMAT "%08x" +#define PX4_CPU_UUID_WORD32_SEPARATOR ":" + +#define PX4_CPU_UUID_WORD32_UNIQUE_H 3 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ + +/* 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 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 */ + +#define px4_spibus_initialize(bus_num_1based) kinetis_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) kinetis_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) kinetis_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) kinetis_pinconfig(pinset) +#define px4_arch_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) kinetis_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) kinetis_gpiowrite(pinset, value) + +/* kinetis_gpiosetevent is not implemented and will need to be added */ + +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) kinetis_gpiosetevent(pinset,r,f,e,fp,a) + +__END_DECLS diff --git a/src/lib/drivers/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/adc/CMakeLists.txt similarity index 97% rename from src/lib/drivers/tone_alarm/CMakeLists.txt rename to platforms/nuttx/src/px4/nxp/kinetis/adc/CMakeLists.txt index 53b343a25e7e..d2487d05bf2c 100644 --- a/src/lib/drivers/tone_alarm/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/kinetis/adc/CMakeLists.txt @@ -31,4 +31,6 @@ # ############################################################################ -# ToneAlarmInterface Library - Intentionally Blank +px4_add_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/nxp/kinetis/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/kinetis/adc/adc.cpp new file mode 100644 index 000000000000..78a650ec937c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/kinetis/adc/adc.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * 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 +#include +#include + + +#define _REG(_addr) (*(volatile uint32_t *)(_addr)) + +/* ADC register accessors */ + +#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg)) + +#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */ +#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */ +#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */ +#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */ +#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */ +#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */ +#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */ +#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */ +#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */ +#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */ +#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */ +#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */ +#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */ +#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */ +#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ +#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ + +int px4_arch_adc_init(uint32_t base_address) +{ + /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + + irqstate_t flags = px4_enter_critical_section(); + + _REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1; + rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8; + rCFG2(1) = 0; + rSC2(1) = ADC_SC2_REFSEL_DEFAULT; + + px4_leave_critical_section(flags); + + /* Clear the CALF and begin the calibration */ + + rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF; + + while ((rSC1A(1) & ADC_SC1_COCO) == 0) { + usleep(100); + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + } + + /* dummy read to clear COCO of calibration */ + + int32_t r = rRA(1); + + /* Check the state of CALF at the end of calibration */ + + if (rSC3(1) & ADC_SC3_CALF) { + return -1; + } + + /* Calculate the calibration values for single ended positive */ + + r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ; + r = 0x8000U | (r >> 1U); + rPG(1) = r; + + /* Calculate the calibration values for double ended Negitive */ + + r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ; + r = 0x8000U | (r >> 1U); + rMG(1) = r; + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + + rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* 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; + } + } + + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + irqstate_t flags = px4_enter_critical_section(); + _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; + px4_leave_critical_section(flags); +} + +uint16_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* clear any previous COCC */ + rRA(1); + + /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ + rSC1A(1) = ADC_SC1_ADCH(channel); + + /* wait for the conversion to complete */ + const hrt_abstime now = hrt_absolute_time(); + + while (!(rSC1A(1) & ADC_SC1_COCO)) { + + /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ + if ((hrt_absolute_time() - now) > 10) { + px4_leave_critical_section(flags); + return 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rRA(1); + + px4_leave_critical_section(flags); + + return result; +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); +} + diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt new file mode 100644 index 000000000000..ebd97f8a42fa --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_hrt + hrt.c +) +target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable + diff --git a/src/drivers/kinetis/drv_hrt.c b/platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c similarity index 100% rename from src/drivers/kinetis/drv_hrt.c rename to platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c diff --git a/src/drivers/stm32/drv_pwm_servo.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h similarity index 90% rename from src/drivers/stm32/drv_pwm_servo.h rename to platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h index ecd42687f6c3..400305ebd21b 100644 --- a/src/drivers/stm32/drv_pwm_servo.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,14 +30,12 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once -/** - * @file drv_pwm_servo.h - * - * stm32-specific PWM output data. - */ +#include -#pragma once +#define SYSTEM_ADC_BASE 0 // not used on kinetis + +#include -#include diff --git a/src/drivers/kinetis/drv_io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h similarity index 100% rename from src/drivers/kinetis/drv_io_timer.h rename to platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/CMakeLists.txt new file mode 100644 index 000000000000..401bf06ef5dd --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_io_pins + io_timer.c + pwm_servo.c + pwm_trigger.c + input_capture.c +) diff --git a/src/drivers/kinetis/drv_input_capture.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c similarity index 99% rename from src/drivers/kinetis/drv_input_capture.c rename to platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c index c44138118d85..ed3bdeaff08c 100644 --- a/src/drivers/kinetis/drv_input_capture.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c @@ -72,9 +72,7 @@ #include #include -#include "drv_io_timer.h" - -#include "drv_input_capture.h" +#include #include #include "chip/kinetis_sim.h" diff --git a/src/drivers/kinetis/drv_io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c similarity index 99% rename from src/drivers/kinetis/drv_io_timer.c rename to platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 4aaa8bb095d5..7ba1ce7fd477 100644 --- a/src/drivers/kinetis/drv_io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -56,7 +56,7 @@ #include #include -#include "drv_io_timer.h" +#include #include #include "chip/kinetis_sim.h" diff --git a/src/drivers/kinetis/drv_pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c similarity index 98% rename from src/drivers/kinetis/drv_pwm_servo.c rename to platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c index be1ab4050d73..908b71225c52 100644 --- a/src/drivers/kinetis/drv_pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c @@ -58,8 +58,7 @@ #include #include -#include "drv_io_timer.h" -#include "drv_pwm_servo.h" +#include #include diff --git a/src/drivers/kinetis/drv_pwm_trigger.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c similarity index 98% rename from src/drivers/kinetis/drv_pwm_trigger.c rename to platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c index 722e32d47450..6c5bc0e92e01 100644 --- a/src/drivers/kinetis/drv_pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c @@ -54,8 +54,7 @@ #include #include -#include "drv_io_timer.h" -#include "drv_pwm_trigger.h" +#include int up_pwm_trigger_set(unsigned channel, uint16_t value) { diff --git a/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/CMakeLists.txt new file mode 100644 index 000000000000..1b1504352612 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_led_pwm + led_pwm.cpp +) diff --git a/src/drivers/kinetis/drv_led_pwm.cpp b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp similarity index 99% rename from src/drivers/kinetis/drv_led_pwm.cpp rename to platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp index b11ca0e4580f..8450d87d72fa 100644 --- a/src/drivers/kinetis/drv_led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/led_pwm/led_pwm.cpp @@ -56,7 +56,7 @@ #include #include -#include "drv_io_timer.h" +#include #include #include "chip/kinetis_sim.h" diff --git a/src/drivers/sim/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/CMakeLists.txt similarity index 97% rename from src/drivers/sim/tone_alarm/CMakeLists.txt rename to platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/CMakeLists.txt index 5251d3910a7a..a2d2f8144bcc 100644 --- a/src/drivers/sim/tone_alarm/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/CMakeLists.txt @@ -31,6 +31,6 @@ # ############################################################################ -px4_add_library(tone_alarm_interface +px4_add_library(arch_tone_alarm ToneAlarmInterface.cpp ) diff --git a/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp similarity index 97% rename from src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp rename to platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp index e7f36bd2ab54..a96180a429ea 100644 --- a/src/drivers/kinetis/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp @@ -39,7 +39,7 @@ #include "kinetis_tpm.h" #include -#include +#include #include #include @@ -115,7 +115,9 @@ # define rCNSC CAT3(rC, TONE_ALARM_CHANNEL, SC) /* Channel Status and Control Register used by Tone alarm */ # define STATUS CAT3(TPM_STATUS_CH, TONE_ALARM_CHANNEL, F) /* Capture and Compare Status Register used by Tone alarm */ -void ToneAlarmInterface::init() +namespace ToneAlarmInterface +{ +void init() { #ifdef GPIO_TONE_ALARM_NEG px4_arch_configgpio(GPIO_TONE_ALARM_NEG); @@ -157,7 +159,7 @@ void ToneAlarmInterface::init() rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this. } -void ToneAlarmInterface::start_note(unsigned frequency) +void start_note(unsigned frequency) { // Calculate the signal switching period. // (Signal switching period is one half of the frequency period). @@ -174,7 +176,7 @@ void ToneAlarmInterface::start_note(unsigned frequency) px4_arch_configgpio(GPIO_TONE_ALARM); } -void ToneAlarmInterface::stop_note() +void stop_note() { // Stop the current note. rSC &= ~TPM_SC_CMOD_MASK; @@ -182,3 +184,5 @@ void ToneAlarmInterface::stop_note() // Ensure the GPIO is not driving the speaker. px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); } + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/stm/CMakeLists.txt b/platforms/nuttx/src/px4/stm/CMakeLists.txt new file mode 100644 index 000000000000..ec489bf559c3 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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/nuttx/src/px4/stm/stm32_common/adc/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/adc/CMakeLists.txt new file mode 100644 index 000000000000..d2487d05bf2c --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/adc/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/adc/adc.cpp b/platforms/nuttx/src/px4/stm/stm32_common/adc/adc.cpp new file mode 100644 index 000000000000..6661c27c1c66 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/adc/adc.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * 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 + +/* + * Register accessors. + * For now, no reason not to just use ADC1. + */ +#define REG(base, _reg) (*(volatile uint32_t *)((base) + (_reg))) + +#define rSR(base) REG((base), STM32_ADC_SR_OFFSET) +#define rCR1(base) REG((base), STM32_ADC_CR1_OFFSET) +#define rCR2(base) REG((base), STM32_ADC_CR2_OFFSET) +#define rSMPR1(base) REG((base), STM32_ADC_SMPR1_OFFSET) +#define rSMPR2(base) REG((base), STM32_ADC_SMPR2_OFFSET) +#define rJOFR1(base) REG((base), STM32_ADC_JOFR1_OFFSET) +#define rJOFR2(base) REG((base), STM32_ADC_JOFR2_OFFSET) +#define rJOFR3(base) REG((base), STM32_ADC_JOFR3_OFFSET) +#define rJOFR4(base) REG((base), STM32_ADC_JOFR4_OFFSET) +#define rHTR(base) REG((base), STM32_ADC_HTR_OFFSET) +#define rLTR(base) REG((base), STM32_ADC_LTR_OFFSET) +#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) +#define rJSQR(base) REG((base), STM32_ADC_JSQR_OFFSET) +#define rJDR1(base) REG((base), STM32_ADC_JDR1_OFFSET) +#define rJDR2(base) REG((base), STM32_ADC_JDR2_OFFSET) +#define rJDR3(base) REG((base), STM32_ADC_JDR3_OFFSET) +#define rJDR4(base) REG((base), STM32_ADC_JDR4_OFFSET) +#define rDR(base) REG((base), STM32_ADC_DR_OFFSET) + + + +#ifdef STM32_ADC_CCR +# define rCCR(base) REG((base), STM32_ADC_CCR_OFFSET) + +/* Assuming VDC 2.4 - 3.6 */ + +#define ADC_MAX_FADC 36000000 + +# if STM32_PCLK2_FREQUENCY/2 <= ADC_MAX_FADC +# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV2 +# elif STM32_PCLK2_FREQUENCY/4 <= ADC_MAX_FADC +# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV4 +# elif STM32_PCLK2_FREQUENCY/6 <= ADC_MAX_FADC +# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV6 +# elif STM32_PCLK2_FREQUENCY/8 <= ADC_MAX_FADC +# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV8 +# else +# error "ADC PCLK2 too high - no divisor found " +# endif +#endif + + +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; + + /* do calibration if supported */ +#ifdef ADC_CR2_CAL + rCR2(base_address) |= ADC_CR2_CAL; + px4_usleep(100); + + if (rCR2(base_address) & ADC_CR2_CAL) { + return -1; + } + +#endif + + /* arbitrarily configure all channels for 55 cycle sample time */ + rSMPR1(base_address) = 0b00000011011011011011011011011011; + rSMPR2(base_address) = 0b00011011011011011011011011011011; + + /* XXX for F2/4, might want to select 12-bit mode? */ + rCR1(base_address) = 0; + + /* enable the temperature sensor / Vrefint channel if supported*/ + rCR2(base_address) = +#ifdef ADC_CR2_TSVREFE + /* enable the temperature sensor in CR2 */ + ADC_CR2_TSVREFE | +#endif + 0; + + /* Soc have CCR */ +#ifdef STM32_ADC_CCR +# ifdef ADC_CCR_TSVREFE + /* enable temperature sensor in CCR */ + rCCR(base_address) = ADC_CCR_TSVREFE | ADC_CCR_ADCPRE_DIV; +# else + rCCR(base_address) = ADC_CCR_ADCPRE_DIV; +# endif +#endif + + /* configure for a single-channel sequence */ + rSQR1(base_address) = 0; + rSQR2(base_address) = 0; + rSQR3(base_address) = 0; /* will be updated with the channel each tick */ + + /* power-cycle the ADC and turn it on */ + rCR2(base_address) &= ~ADC_CR2_ADON; + px4_usleep(10); + rCR2(base_address) |= ADC_CR2_ADON; + px4_usleep(10); + rCR2(base_address) |= ADC_CR2_ADON; + px4_usleep(10); + + /* kick off a sample and wait for it to complete */ + hrt_abstime now = hrt_absolute_time(); + rCR2(base_address) |= ADC_CR2_SWSTART; + + while (!(rSR(base_address) & ADC_SR_EOC)) { + + /* 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; + } + } + + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + // nothing to do +} + +uint16_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* clear any previous EOC */ + if (rSR(base_address) & ADC_SR_EOC) { + rSR(base_address) &= ~ADC_SR_EOC; + } + + /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ + rSQR3(base_address) = channel; + rCR2(base_address) |= ADC_CR2_SWSTART; + + /* wait for the conversion to complete */ + const hrt_abstime now = hrt_absolute_time(); + + while (!(rSR(base_address) & ADC_SR_EOC)) { + + /* 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 0xffff; + } + } + + /* read the result and clear EOC */ + uint16_t result = rDR(base_address); + + px4_leave_critical_section(flags); + + return result; +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + return 1 << 16; +} + diff --git a/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt new file mode 100644 index 000000000000..ebd97f8a42fa --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/hrt/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_hrt + hrt.c +) +target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable + diff --git a/src/drivers/stm32/drv_hrt.c b/platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c similarity index 100% rename from src/drivers/stm32/drv_hrt.c rename to platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c diff --git a/src/platforms/px4_middleware.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h similarity index 71% rename from src/platforms/px4_middleware.h rename to platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h index 393074848799..17ff58304537 100644 --- a/src/platforms/px4_middleware.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,38 +30,28 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - -/** - * @file px4_middleware.h - * - * PX4 generic middleware wrapper - */ - #pragma once -#include -#include +#include -namespace px4 -{ -__EXPORT void init(int argc, char *argv[], const char *process_name); +/* Historically PX4 used one ADC1 With FMUvnX this has changes. + * These defines maintain compatibility while allowing the + * new boards to override the ADC used from HW VER/REV and + * the system one. + * + * Depending on HW configuration (VER/REV POP options) hardware detection + * may or may NOT initialize a given ADC. SYSTEM_ADC_COUNT is used to size the + * singleton array to ensure this is only done once per ADC. + */ -__EXPORT uint64_t get_time_micros(); +#if !defined(HW_REV_VER_ADC_BASE) +# define HW_REV_VER_ADC_BASE STM32_ADC1_BASE +#endif -#if defined(__PX4_NUTTX) -extern bool task_should_exit; -/** - * Returns true if the app/task should continue to run - */ -__EXPORT inline bool ok() { return !task_should_exit; } -#elif defined(__PX4_QURT) -// FIXME - usleep not supported by DSPAL -inline void usleep(uint64_t sleep_interval) { } -#else -/** - * Linux needs to have globally unique checks for thread/task status - */ +#if !defined(SYSTEM_ADC_BASE) +# define SYSTEM_ADC_BASE STM32_ADC1_BASE #endif -} // namespace px4 +#include + diff --git a/src/drivers/stm32/drv_io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h similarity index 100% rename from src/drivers/stm32/drv_io_timer.h rename to platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h 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 new file mode 100644 index 000000000000..6b1bb58216cf --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/micro_hal.h @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 + +__BEGIN_DECLS + +#include +#include +#include + +/* STM32/32F7 defines the 96 bit UUID as + * init32_t[3] that can be read as bytes/half-words/words + * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 31:0 (offset 0) + * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 4) + * init32_t[2] PX4_CPU_UUID_ADDRESS[3] bits 96:64 (offset 8) + * + * The original PX4 stm32 (legacy) based implementation **displayed** the + * UUID as: ABCD EFGH IJKL + * Where: + * A was bit 31 and D was bit 0 + * E was bit 63 and H was bit 32 + * I was bit 95 and L was bit 64 + * + * Since the string was used by some manufactures to identify the units + * it must be preserved. + * + * For new targets moving forward we will use + * IJKL EFGH ABCD + */ +#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) stm32_bbsram_savepanic(fileno, context, length) + +#define PX4_BUS_OFFSET 0 /* STM buses are 1 based no adjustment needed */ +#define px4_spibus_initialize(bus_num_1based) stm32_spibus_initialize(bus_num_1based) + +#define px4_i2cbus_initialize(bus_num_1based) stm32_i2cbus_initialize(bus_num_1based) +#define px4_i2cbus_uninitialize(pdev) stm32_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) stm32_configgpio(pinset) +#define px4_arch_unconfiggpio(pinset) stm32_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) stm32_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) stm32_gpiowrite(pinset, value) +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) stm32_gpiosetevent(pinset,r,f,e,fp,a) + +__END_DECLS diff --git a/src/drivers/px4io/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h similarity index 65% rename from src/drivers/px4io/px4io_serial.h rename to platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index 8e17606da68e..f8888f402419 100644 --- a/src/drivers/px4io/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -32,32 +32,18 @@ ****************************************************************************/ /** - * @file px4io_driver.h + * @file px4io_serial.h * - * Interface for PX4IO + * Serial Interface definition for PX4IO */ #pragma once -/* XXX trim includes */ +#include #include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include - #include #include @@ -117,87 +103,17 @@ class PX4IO_serial : public device::Device PX4IO_serial &operator = (const PX4IO_serial &); }; -#if defined(CONFIG_STM32_STM32F10XX) || defined(CONFIG_STM32_STM32F4XXX) -/** XXX use F4 implementation for F1 as well. **/ - -#define PX4IO_INTERFACE_CLASS PX4IO_serial_f4 -#define PX4IO_INTERFACE_F4 - -class PX4IO_serial_f4 : public PX4IO_serial -{ -public: - PX4IO_serial_f4(); - ~PX4IO_serial_f4(); - - virtual int init(); - virtual int ioctl(unsigned operation, unsigned &arg); - -protected: - /** - * Start the transaction with IO and wait for it to complete. - */ - int _bus_exchange(IOPacket *_packet); - -private: - DMA_HANDLE _tx_dma; - DMA_HANDLE _rx_dma; - - IOPacket *_current_packet; - - /** saved DMA status */ - static const unsigned _dma_status_inactive = 0x80000000; // low bits overlap DMA_STATUS_* values - static const unsigned _dma_status_waiting = 0x00000000; - volatile unsigned _rx_dma_status; - - /** client-waiting lock/signal */ - px4_sem_t _completion_semaphore; - - /** - * DMA completion handler. - */ - static void _dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); - void _do_rx_dma_callback(unsigned status); - - /** - * Serial interrupt handler. - */ - static int _interrupt(int vector, void *context, void *arg); - void _do_interrupt(); - - /** - * Cancel any DMA in progress with an error. - */ - void _abort_dma(); - - /** - * Performance counters. - */ - perf_counter_t _pc_dmasetup; - perf_counter_t _pc_dmaerrs; - - /* do not allow top copying this class */ - PX4IO_serial_f4(PX4IO_serial_f4 &); - PX4IO_serial_f4 &operator = (const PX4IO_serial_f4 &); - - /** - * IO Buffer storage - */ - static IOPacket _io_buffer_storage; // XXX static to ensure DMA-able memory -}; - -#elif defined(CONFIG_ARCH_CHIP_STM32F7) - -#define PX4IO_INTERFACE_CLASS PX4IO_serial_f7 -#define PX4IO_INTERFACE_F7 #include -class PX4IO_serial_f7 : public PX4IO_serial +class ArchPX4IOSerial : public PX4IO_serial { public: - PX4IO_serial_f7(); - ~PX4IO_serial_f7(); + ArchPX4IOSerial(); + ArchPX4IOSerial(ArchPX4IOSerial &) = delete; + ArchPX4IOSerial &operator = (const ArchPX4IOSerial &) = delete; + ~ArchPX4IOSerial(); virtual int init(); virtual int ioctl(unsigned operation, unsigned &arg); @@ -245,16 +161,9 @@ class PX4IO_serial_f7 : public PX4IO_serial perf_counter_t _pc_dmasetup; perf_counter_t _pc_dmaerrs; - /* do not allow top copying this class */ - PX4IO_serial_f7(PX4IO_serial_f7 &); - PX4IO_serial_f7 &operator = (const PX4IO_serial_f7 &); - /** * IO Buffer storage */ - static uint8_t _io_buffer_storage[] __attribute__((aligned(ARMV7M_DCACHE_LINESIZE))); + static uint8_t _io_buffer_storage[] __attribute__((aligned(PX4IO_SERIAL_BUF_ALIGN))); }; -#else -#error "Interface not implemented for this chip" -#endif 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 new file mode 100644 index 000000000000..401bf06ef5dd --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_io_pins + io_timer.c + pwm_servo.c + pwm_trigger.c + input_capture.c +) diff --git a/src/drivers/stm32/drv_input_capture.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c similarity index 99% rename from src/drivers/stm32/drv_input_capture.c rename to platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c index c7505087b1b6..a1eb6a9743a0 100644 --- a/src/drivers/stm32/drv_input_capture.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/input_capture.c @@ -72,9 +72,7 @@ #include #include -#include "drv_io_timer.h" - -#include "drv_input_capture.h" +#include #include #include diff --git a/src/drivers/stm32/drv_io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c similarity index 99% rename from src/drivers/stm32/drv_io_timer.c rename to platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 4f654e8a6762..7a11dec0adbc 100644 --- a/src/drivers/stm32/drv_io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -58,7 +58,7 @@ #include #include -#include "drv_io_timer.h" +#include #include #include diff --git a/src/drivers/stm32/drv_pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c similarity index 98% rename from src/drivers/stm32/drv_pwm_servo.c rename to platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c index b75772ebc418..9121ea488b14 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c @@ -58,8 +58,7 @@ #include #include -#include "drv_io_timer.h" -#include "drv_pwm_servo.h" +#include #include diff --git a/src/drivers/stm32/drv_pwm_trigger.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c similarity index 98% rename from src/drivers/stm32/drv_pwm_trigger.c rename to platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c index 86f38acf3964..477206d25fe0 100644 --- a/src/drivers/stm32/drv_pwm_trigger.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_trigger.c @@ -54,8 +54,7 @@ #include #include -#include "drv_io_timer.h" -#include "drv_pwm_trigger.h" +#include #include diff --git a/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/CMakeLists.txt new file mode 100644 index 000000000000..1b1504352612 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_led_pwm + led_pwm.cpp +) diff --git a/src/drivers/stm32/drv_led_pwm.cpp b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp similarity index 99% rename from src/drivers/stm32/drv_led_pwm.cpp rename to platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp index c9eafb239171..46cfceeb478a 100644 --- a/src/drivers/stm32/drv_led_pwm.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/led_pwm/led_pwm.cpp @@ -57,7 +57,7 @@ #include #include -#include +#include diff --git a/src/drivers/kinetis/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/CMakeLists.txt similarity index 97% rename from src/drivers/kinetis/tone_alarm/CMakeLists.txt rename to platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/CMakeLists.txt index 5251d3910a7a..a2d2f8144bcc 100644 --- a/src/drivers/kinetis/tone_alarm/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/CMakeLists.txt @@ -31,6 +31,6 @@ # ############################################################################ -px4_add_library(tone_alarm_interface +px4_add_library(arch_tone_alarm ToneAlarmInterface.cpp ) diff --git a/src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterface.cpp similarity index 100% rename from src/drivers/stm32/tone_alarm/ToneAlarmInterface.cpp rename to platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterface.cpp diff --git a/src/drivers/stm32/tone_alarm/ToneAlarmInterfaceGPIO.cpp b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp similarity index 91% rename from src/drivers/stm32/tone_alarm/ToneAlarmInterfaceGPIO.cpp rename to platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp index c73a51296379..c1d9603e9bb1 100644 --- a/src/drivers/stm32/tone_alarm/ToneAlarmInterfaceGPIO.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp @@ -31,23 +31,28 @@ * ****************************************************************************/ -#include +#include #include #include -void ToneAlarmInterface::init() +namespace ToneAlarmInterface +{ + +void init() { // Configure the GPIO to the idle state. px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); } -void ToneAlarmInterface::start_note(unsigned frequency) +void start_note(unsigned frequency) { px4_arch_gpiowrite(GPIO_TONE_ALARM_GPIO, 1); } -void ToneAlarmInterface::stop_note() +void stop_note() { // Stop the current note. px4_arch_gpiowrite(GPIO_TONE_ALARM_GPIO, 0); } + +} /* namespace ToneAlarmInterface */ diff --git a/src/drivers/stm32/tone_alarm/ToneAlarmInterfacePWM.cpp b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp similarity index 98% rename from src/drivers/stm32/tone_alarm/ToneAlarmInterfacePWM.cpp rename to platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp index eb72700a0adc..da29ce28f487 100644 --- a/src/drivers/stm32/tone_alarm/ToneAlarmInterfacePWM.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ #include -#include +#include #include #include @@ -219,7 +219,10 @@ # define rSR REG(STM32_GTIM_SR_OFFSET) #endif -void ToneAlarmInterface::init() +namespace ToneAlarmInterface +{ + +void init() { #ifdef GPIO_TONE_ALARM_NEG px4_arch_configgpio(GPIO_TONE_ALARM_NEG); @@ -251,7 +254,7 @@ void ToneAlarmInterface::init() rCR1 = GTIM_CR1_CEN; // Ensure the timer is running. } -void ToneAlarmInterface::start_note(unsigned frequency) +void start_note(unsigned frequency) { // Calculate the signal switching period. // (Signal switching period is one half of the frequency period). @@ -276,7 +279,7 @@ void ToneAlarmInterface::start_note(unsigned frequency) px4_arch_configgpio(GPIO_TONE_ALARM); } -void ToneAlarmInterface::stop_note() +void stop_note() { // Stop the current note. rCCER &= ~TONE_CCER; @@ -284,3 +287,5 @@ void ToneAlarmInterface::stop_note() // Ensure the GPIO is not driving the speaker. px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); } + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt new file mode 100644 index 000000000000..ead07409b1af --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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(../stm32_common/io_pins io_pins) +add_subdirectory(../stm32_common/hrt hrt) diff --git a/src/drivers/kinetis/drv_pwm_trigger.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/io_timer.h similarity index 90% rename from src/drivers/kinetis/drv_pwm_trigger.h rename to platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/io_timer.h index e27902f51455..2960835d2ec2 100644 --- a/src/drivers/kinetis/drv_pwm_trigger.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,13 +30,8 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once -/** - * @file drv_pwm_trigger.h - * - * stm32-specific PWM output data. - */ -#pragma once +#include "../../../stm32_common/include/px4_arch/io_timer.h" -#include diff --git a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..2c06c7641c3d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/micro_hal.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#include +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F4 +#define PX4_FLASH_BASE STM32_FLASH_BASE +#if defined(CONFIG_STM32_STM32F4XXX) +# include +# define PX4_BBSRAM_SIZE STM32_BBSRAM_SIZE +# define PX4_BBSRAM_GETDESC_IOCTL STM32_BBSRAM_GETDESC_IOCTL +#endif +#define PX4_NUMBER_I2C_BUSES STM32_NI2C + +__END_DECLS + diff --git a/src/drivers/stm32/drv_pwm_trigger.h b/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h similarity index 90% rename from src/drivers/stm32/drv_pwm_trigger.h rename to platforms/nuttx/src/px4/stm/stm32f1/io_timer.h index e27902f51455..2960835d2ec2 100644 --- a/src/drivers/stm32/drv_pwm_trigger.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,13 +30,8 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once -/** - * @file drv_pwm_trigger.h - * - * stm32-specific PWM output data. - */ -#pragma once +#include "../../../stm32_common/include/px4_arch/io_timer.h" -#include diff --git a/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt new file mode 100644 index 000000000000..40432707c786 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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(../stm32_common/hrt hrt) + + diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h new file mode 100644 index 000000000000..9aa5b0c7aeba --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/adc.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..2960835d2ec2 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/io_timer.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..2c06c7641c3d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#include +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F4 +#define PX4_FLASH_BASE STM32_FLASH_BASE +#if defined(CONFIG_STM32_STM32F4XXX) +# include +# define PX4_BBSRAM_SIZE STM32_BBSRAM_SIZE +# define PX4_BBSRAM_GETDESC_IOCTL STM32_BBSRAM_GETDESC_IOCTL +#endif +#define PX4_NUMBER_I2C_BUSES STM32_NI2C + +__END_DECLS + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt new file mode 100644 index 000000000000..0b5fe399e0b7 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(../stm32_common/adc adc) +add_subdirectory(../stm32_common/hrt hrt) +add_subdirectory(../stm32_common/led_pwm led_pwm) +add_subdirectory(../stm32_common/io_pins io_pins) +add_subdirectory(../stm32_common/tone_alarm tone_alarm) + +add_subdirectory(px4io_serial) + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h new file mode 100644 index 000000000000..9aa5b0c7aeba --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/adc.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..2960835d2ec2 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/io_timer.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/io_timer.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..2c06c7641c3d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#include +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F4 +#define PX4_FLASH_BASE STM32_FLASH_BASE +#if defined(CONFIG_STM32_STM32F4XXX) +# include +# define PX4_BBSRAM_SIZE STM32_BBSRAM_SIZE +# define PX4_BBSRAM_GETDESC_IOCTL STM32_BBSRAM_GETDESC_IOCTL +#endif +#define PX4_NUMBER_I2C_BUSES STM32_NI2C + +__END_DECLS + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h new file mode 100644 index 000000000000..77564d2ad09f --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +#define PX4IO_SERIAL_BUF_ALIGN 4 +#include "../../../stm32_common/include/px4_arch/px4io_serial.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/CMakeLists.txt new file mode 100644 index 000000000000..df450593b1b8 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_px4io_serial + px4io_serial.cpp +) diff --git a/src/drivers/px4io/px4io_serial_f4.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp similarity index 93% rename from src/drivers/px4io/px4io_serial_f4.cpp rename to platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp index 8ada27f0a1c2..8b70c62dc473 100644 --- a/src/drivers/px4io/px4io_serial_f4.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp @@ -32,14 +32,12 @@ ****************************************************************************/ /** - * @file px4io_serial_f4.cpp + * @file px4io_serial.cpp * * Serial interface for PX4IO on STM32F4 */ -#include "px4io_serial.h" - -#ifdef PX4IO_INTERFACE_F4 +#include /* serial register accessors */ #define REG(_x) (*(volatile uint32_t *)(PX4IO_SERIAL_BASE + _x)) @@ -51,9 +49,9 @@ #define rCR3 REG(STM32_USART_CR3_OFFSET) #define rGTPR REG(STM32_USART_GTPR_OFFSET) -IOPacket PX4IO_serial_f4::_io_buffer_storage; +uint8_t ArchPX4IOSerial::_io_buffer_storage[sizeof(IOPacket)]; -PX4IO_serial_f4::PX4IO_serial_f4() : +ArchPX4IOSerial::ArchPX4IOSerial() : _tx_dma(nullptr), _rx_dma(nullptr), _current_packet(nullptr), @@ -69,7 +67,7 @@ PX4IO_serial_f4::PX4IO_serial_f4() : { } -PX4IO_serial_f4::~PX4IO_serial_f4() +ArchPX4IOSerial::~ArchPX4IOSerial() { if (_tx_dma != nullptr) { stm32_dmastop(_tx_dma); @@ -105,12 +103,12 @@ PX4IO_serial_f4::~PX4IO_serial_f4() } int -PX4IO_serial_f4::init() +ArchPX4IOSerial::init() { /* initialize base implementation */ int r; - if ((r = PX4IO_serial::init(&_io_buffer_storage)) != 0) { + if ((r = PX4IO_serial::init((IOPacket *)&_io_buffer_storage[0])) != 0) { return r; } @@ -167,7 +165,7 @@ PX4IO_serial_f4::init() } int -PX4IO_serial_f4::ioctl(unsigned operation, unsigned &arg) +ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) { switch (operation) { @@ -232,7 +230,7 @@ PX4IO_serial_f4::ioctl(unsigned operation, unsigned &arg) } int -PX4IO_serial_f4::_bus_exchange(IOPacket *_packet) +ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { _current_packet = _packet; @@ -351,17 +349,17 @@ PX4IO_serial_f4::_bus_exchange(IOPacket *_packet) } void -PX4IO_serial_f4::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +ArchPX4IOSerial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { if (arg != nullptr) { - PX4IO_serial_f4 *ps = reinterpret_cast(arg); + ArchPX4IOSerial *ps = reinterpret_cast(arg); ps->_do_rx_dma_callback(status); } } void -PX4IO_serial_f4::_do_rx_dma_callback(unsigned status) +ArchPX4IOSerial::_do_rx_dma_callback(unsigned status) { /* on completion of a reply, wake the waiter */ if (_rx_dma_status == _dma_status_waiting) { @@ -386,10 +384,10 @@ PX4IO_serial_f4::_do_rx_dma_callback(unsigned status) } int -PX4IO_serial_f4::_interrupt(int irq, void *context, void *arg) +ArchPX4IOSerial::_interrupt(int irq, void *context, void *arg) { if (arg != nullptr) { - PX4IO_serial_f4 *instance = reinterpret_cast(arg); + ArchPX4IOSerial *instance = reinterpret_cast(arg); instance->_do_interrupt(); } @@ -398,7 +396,7 @@ PX4IO_serial_f4::_interrupt(int irq, void *context, void *arg) } void -PX4IO_serial_f4::_do_interrupt() +ArchPX4IOSerial::_do_interrupt() { uint32_t sr = rSR; /* get UART status register */ (void)rDR; /* read DR to clear status */ @@ -458,7 +456,7 @@ PX4IO_serial_f4::_do_interrupt() } void -PX4IO_serial_f4::_abort_dma() +ArchPX4IOSerial::_abort_dma() { /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); @@ -471,4 +469,3 @@ PX4IO_serial_f4::_abort_dma() stm32_dmastop(_rx_dma); } -#endif /* PX4IO_INTERFACE_F4 */ diff --git a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt new file mode 100644 index 000000000000..0b5fe399e0b7 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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(../stm32_common/adc adc) +add_subdirectory(../stm32_common/hrt hrt) +add_subdirectory(../stm32_common/led_pwm led_pwm) +add_subdirectory(../stm32_common/io_pins io_pins) +add_subdirectory(../stm32_common/tone_alarm tone_alarm) + +add_subdirectory(px4io_serial) + diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h new file mode 100644 index 000000000000..9aa5b0c7aeba --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/adc.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h new file mode 100644 index 000000000000..2960835d2ec2 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * 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 "../../../stm32_common/include/px4_arch/io_timer.h" + diff --git a/src/drivers/imu/icm20948/accel.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h similarity index 71% rename from src/drivers/imu/icm20948/accel.h rename to platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h index ea1732b6a87b..af9a59589596 100644 --- a/src/drivers/imu/icm20948/accel.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,38 +30,26 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - #pragma once -#include -#include -#include - -class ICM20948; - -/** - * Helper class implementing the accel driver node. - */ -class ICM20948_accel : public device::CDev -{ -public: - ICM20948_accel(ICM20948 *parent, const char *path); - ~ICM20948_accel(); - - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual int init(); +#include "../../../stm32_common/include/px4_arch/micro_hal.h" -protected: - friend class ICM20948; +__BEGIN_DECLS - void parent_poll_notify(); +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F7 +#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 +#define PX4_FLASH_BASE 0x08000000 +#define PX4_NUMBER_I2C_BUSES STM32F7_NI2C -private: - ICM20948 *_parent; +void stm32_flash_lock(void); +void stm32_flash_unlock(void); +int stm32_flash_writeprotect(size_t page, bool enabled); - orb_advert_t _accel_topic{nullptr}; - int _accel_orb_class_instance{-1}; - int _accel_class_instance{-1}; +__END_DECLS -}; diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h new file mode 100644 index 000000000000..0e78048670cc --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * 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 + +#define PX4IO_SERIAL_BUF_ALIGN ARMV7M_DCACHE_LINESIZE +#include "../../../stm32_common/include/px4_arch/px4io_serial.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/CMakeLists.txt new file mode 100644 index 000000000000..df450593b1b8 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_px4io_serial + px4io_serial.cpp +) diff --git a/src/drivers/px4io/px4io_serial_f7.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp similarity index 94% rename from src/drivers/px4io/px4io_serial_f7.cpp rename to platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp index bcaa38f471a3..0161736a7ba3 100644 --- a/src/drivers/px4io/px4io_serial_f7.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp @@ -32,14 +32,12 @@ ****************************************************************************/ /** - * @file px4io_serial_f7.cpp + * @file px4io_serial.cpp * * Serial interface for PX4IO on STM32F7 */ -#include "px4io_serial.h" - -#ifdef PX4IO_INTERFACE_F7 +#include #include "stm32_uart.h" #include @@ -60,9 +58,9 @@ #define DMA_BUFFER_MASK (ARMV7M_DCACHE_LINESIZE - 1) #define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) -uint8_t PX4IO_serial_f7::_io_buffer_storage[DMA_ALIGN_UP(sizeof(IOPacket))]; +uint8_t ArchPX4IOSerial::_io_buffer_storage[DMA_ALIGN_UP(sizeof(IOPacket))]; -PX4IO_serial_f7::PX4IO_serial_f7() : +ArchPX4IOSerial::ArchPX4IOSerial() : _tx_dma(nullptr), _rx_dma(nullptr), _current_packet(nullptr), @@ -78,7 +76,7 @@ PX4IO_serial_f7::PX4IO_serial_f7() : { } -PX4IO_serial_f7::~PX4IO_serial_f7() +ArchPX4IOSerial::~ArchPX4IOSerial() { if (_tx_dma != nullptr) { stm32_dmastop(_tx_dma); @@ -114,7 +112,7 @@ PX4IO_serial_f7::~PX4IO_serial_f7() } int -PX4IO_serial_f7::init() +ArchPX4IOSerial::init() { /* initialize base implementation */ int r = PX4IO_serial::init((IOPacket *)&_io_buffer_storage[0]); @@ -178,7 +176,7 @@ PX4IO_serial_f7::init() } int -PX4IO_serial_f7::ioctl(unsigned operation, unsigned &arg) +ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) { switch (operation) { @@ -243,7 +241,7 @@ PX4IO_serial_f7::ioctl(unsigned operation, unsigned &arg) } int -PX4IO_serial_f7::_bus_exchange(IOPacket *_packet) +ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) { _current_packet = _packet; @@ -369,17 +367,17 @@ PX4IO_serial_f7::_bus_exchange(IOPacket *_packet) } void -PX4IO_serial_f7::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +ArchPX4IOSerial::_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) { if (arg != nullptr) { - PX4IO_serial_f7 *ps = reinterpret_cast(arg); + ArchPX4IOSerial *ps = reinterpret_cast(arg); ps->_do_rx_dma_callback(status); } } void -PX4IO_serial_f7::_do_rx_dma_callback(unsigned status) +ArchPX4IOSerial::_do_rx_dma_callback(unsigned status) { /* on completion of a reply, wake the waiter */ if (_rx_dma_status == _dma_status_waiting) { @@ -405,10 +403,10 @@ PX4IO_serial_f7::_do_rx_dma_callback(unsigned status) } int -PX4IO_serial_f7::_interrupt(int irq, void *context, void *arg) +ArchPX4IOSerial::_interrupt(int irq, void *context, void *arg) { if (arg != nullptr) { - PX4IO_serial_f7 *instance = reinterpret_cast(arg); + ArchPX4IOSerial *instance = reinterpret_cast(arg); instance->_do_interrupt(); } @@ -417,7 +415,7 @@ PX4IO_serial_f7::_interrupt(int irq, void *context, void *arg) } void -PX4IO_serial_f7::_do_interrupt() +ArchPX4IOSerial::_do_interrupt() { uint32_t sr = rISR; /* get UART status register */ @@ -485,7 +483,7 @@ PX4IO_serial_f7::_do_interrupt() } void -PX4IO_serial_f7::_abort_dma() +ArchPX4IOSerial::_abort_dma() { /* stop DMA */ stm32_dmastop(_tx_dma); @@ -502,4 +500,3 @@ PX4IO_serial_f7::_abort_dma() rICR = rISR & rISR_ERR_FLAGS_MASK; /* clear the flags */ } -#endif /* PX4IO_INTERFACE_F7 */ diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 3ba23f6123ea..131bbb305c4d 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -1,6 +1,4 @@ -add_subdirectory(src) - include_directories(${CMAKE_CURRENT_BINARY_DIR}) get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) @@ -37,7 +35,7 @@ if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlfl APPS_DEST "/home/linaro" SOURCES px4muorb_stub.c - src/main.cpp + src/px4/common/main.cpp apps.cpp LINK_LIBS -Wl,--start-group @@ -52,7 +50,7 @@ if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlfl else() add_executable(px4 - src/main.cpp + src/px4/common/main.cpp apps.cpp ) diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake index da86c70ce878..cbee631e3be5 100644 --- a/platforms/posix/cmake/px4_impl_os.cmake +++ b/platforms/posix/cmake/px4_impl_os.cmake @@ -42,6 +42,7 @@ # Required OS Interface Functions # # * px4_os_add_flags +# * px4_os_determine_build_chip # * px4_os_prebuild_targets # @@ -98,8 +99,8 @@ function(px4_posix_generate_builtin_commands) math(EXPR command_count "${command_count}+1") endif() endforeach() - configure_file(${PX4_SOURCE_DIR}/src/platforms/apps.cpp.in ${OUT}.cpp) - configure_file(${PX4_SOURCE_DIR}/src/platforms/apps.h.in ${OUT}.h) + 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() @@ -149,7 +150,7 @@ function(px4_posix_generate_alias) ) endif() endforeach() - configure_file(${PX4_SOURCE_DIR}/platforms/posix/src/px4-alias.sh_in ${OUT}) + configure_file(${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/px4-alias.sh_in ${OUT}) endfunction() @@ -325,6 +326,23 @@ function(px4_os_add_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 diff --git a/platforms/posix/src/px4/CMakeLists.txt b/platforms/posix/src/px4/CMakeLists.txt new file mode 100644 index 000000000000..929788b587e2 --- /dev/null +++ b/platforms/posix/src/px4/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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/posix/src/px4_layer/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt similarity index 92% rename from platforms/posix/src/px4_layer/CMakeLists.txt rename to platforms/posix/src/px4/common/CMakeLists.txt index ffa06384614d..491f56bea62a 100644 --- a/platforms/posix/src/px4_layer/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -31,6 +31,10 @@ # ############################################################################ + +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") @@ -56,7 +60,7 @@ add_library(px4_layer target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable target_link_libraries(px4_layer PRIVATE work_queue px4_work_queue) -target_link_libraries(px4_layer PRIVATE px4_daemon) +target_link_libraries(px4_layer PRIVATE px4_daemon drivers_board) if(ENABLE_LOCKSTEP_SCHEDULER) target_link_libraries(px4_layer PRIVATE lockstep_scheduler) @@ -66,3 +70,10 @@ endif() if (EXTRA_DEPENDS) add_dependencies(px4_layer ${EXTRA_DEPENDS}) endif() + + +if (BUILD_TESTING) + add_subdirectory(test_stubs) + add_subdirectory(gtest_runner) +endif() + diff --git a/platforms/posix/src/px4_layer/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp similarity index 100% rename from platforms/posix/src/px4_layer/drv_hrt.cpp rename to platforms/posix/src/px4/common/drv_hrt.cpp diff --git a/platforms/posix/src/px4/common/gtest_runner/CMakeLists.txt b/platforms/posix/src/px4/common/gtest_runner/CMakeLists.txt new file mode 100644 index 000000000000..0000dda9f8ae --- /dev/null +++ b/platforms/posix/src/px4/common/gtest_runner/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(SRCS + gtest_functional_main.cpp + ) + +px4_add_library(gtest_functional_main ${SRCS}) +target_link_libraries(gtest_functional_main PUBLIC gtest) diff --git a/src/drivers/kinetis/drv_input_capture.h b/platforms/posix/src/px4/common/gtest_runner/gtest_functional_main.cpp similarity index 86% rename from src/drivers/kinetis/drv_input_capture.h rename to platforms/posix/src/px4/common/gtest_runner/gtest_functional_main.cpp index 968ce831efaf..dc44167a34e2 100644 --- a/src/drivers/kinetis/drv_input_capture.h +++ b/platforms/posix/src/px4/common/gtest_runner/gtest_functional_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2016 PX4 Development Team. All rights reserved. + * 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 @@ -31,12 +31,15 @@ * ****************************************************************************/ -/** - * @file drv_input_capture.h - * - * stm32-specific input capture data. - */ +#include + +#include -#pragma once +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); -#include + uORB::Manager::initialize(); + param_init(); + return RUN_ALL_TESTS(); +} diff --git a/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h b/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h new file mode 100644 index 000000000000..a629cda2f956 --- /dev/null +++ b/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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 + + + diff --git a/platforms/posix/src/px4_layer/lib_crc32.c b/platforms/posix/src/px4/common/lib_crc32.c similarity index 100% rename from platforms/posix/src/px4_layer/lib_crc32.c rename to platforms/posix/src/px4/common/lib_crc32.c diff --git a/platforms/posix/src/lockstep_scheduler/.gitignore b/platforms/posix/src/px4/common/lockstep_scheduler/.gitignore similarity index 100% rename from platforms/posix/src/lockstep_scheduler/.gitignore rename to platforms/posix/src/px4/common/lockstep_scheduler/.gitignore diff --git a/platforms/posix/src/lockstep_scheduler/CMakeLists.txt b/platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt similarity index 75% rename from platforms/posix/src/lockstep_scheduler/CMakeLists.txt rename to platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt index 0705c07278a1..da84323d9d72 100644 --- a/platforms/posix/src/lockstep_scheduler/CMakeLists.txt +++ b/platforms/posix/src/px4/common/lockstep_scheduler/CMakeLists.txt @@ -10,4 +10,4 @@ target_include_directories(lockstep_scheduler ${CMAKE_CURRENT_SOURCE_DIR}/include ) -px4_add_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler) +px4_add_unit_gtest(SRC test/src/lockstep_scheduler_test.cpp LINKLIBS lockstep_scheduler) diff --git a/platforms/posix/src/lockstep_scheduler/build-and-test.sh b/platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh similarity index 100% rename from platforms/posix/src/lockstep_scheduler/build-and-test.sh rename to platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh diff --git a/platforms/posix/src/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h similarity index 100% rename from platforms/posix/src/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h rename to platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h diff --git a/platforms/posix/src/lockstep_scheduler/src/lockstep_scheduler.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp similarity index 100% rename from platforms/posix/src/lockstep_scheduler/src/lockstep_scheduler.cpp rename to platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp diff --git a/platforms/posix/src/lockstep_scheduler/test/CMakeLists.txt b/platforms/posix/src/px4/common/lockstep_scheduler/test/CMakeLists.txt similarity index 100% rename from platforms/posix/src/lockstep_scheduler/test/CMakeLists.txt rename to platforms/posix/src/px4/common/lockstep_scheduler/test/CMakeLists.txt diff --git a/platforms/posix/src/lockstep_scheduler/test/src/lockstep_scheduler_test.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/test/src/lockstep_scheduler_test.cpp similarity index 100% rename from platforms/posix/src/lockstep_scheduler/test/src/lockstep_scheduler_test.cpp rename to platforms/posix/src/px4/common/lockstep_scheduler/test/src/lockstep_scheduler_test.cpp diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/px4/common/main.cpp similarity index 99% rename from platforms/posix/src/main.cpp rename to platforms/posix/src/px4/common/main.cpp index 21763ae2afa4..016969031a22 100644 --- a/platforms/posix/src/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -64,14 +64,13 @@ #include #include +#include #include #include #include #include "apps.h" -#include "px4_middleware.h" #include "DriverFramework.hpp" -#include "px4_middleware.h" #include "px4_daemon/client.h" #include "px4_daemon/server.h" #include "px4_daemon/pxh.h" diff --git a/platforms/posix/src/px4-alias.sh_in b/platforms/posix/src/px4/common/px4-alias.sh_in similarity index 100% rename from platforms/posix/src/px4-alias.sh_in rename to platforms/posix/src/px4/common/px4-alias.sh_in diff --git a/platforms/posix/src/px4_daemon/CMakeLists.txt b/platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt similarity index 100% rename from platforms/posix/src/px4_daemon/CMakeLists.txt rename to platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt diff --git a/platforms/posix/src/px4_daemon/client.cpp b/platforms/posix/src/px4/common/px4_daemon/client.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/client.cpp rename to platforms/posix/src/px4/common/px4_daemon/client.cpp diff --git a/platforms/posix/src/px4_daemon/client.h b/platforms/posix/src/px4/common/px4_daemon/client.h similarity index 100% rename from platforms/posix/src/px4_daemon/client.h rename to platforms/posix/src/px4/common/px4_daemon/client.h diff --git a/platforms/posix/src/px4_daemon/history.cpp b/platforms/posix/src/px4/common/px4_daemon/history.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/history.cpp rename to platforms/posix/src/px4/common/px4_daemon/history.cpp diff --git a/platforms/posix/src/px4_daemon/history.h b/platforms/posix/src/px4/common/px4_daemon/history.h similarity index 100% rename from platforms/posix/src/px4_daemon/history.h rename to platforms/posix/src/px4/common/px4_daemon/history.h diff --git a/platforms/posix/src/px4_daemon/pxh.cpp b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/pxh.cpp rename to platforms/posix/src/px4/common/px4_daemon/pxh.cpp diff --git a/platforms/posix/src/px4_daemon/pxh.h b/platforms/posix/src/px4/common/px4_daemon/pxh.h similarity index 100% rename from platforms/posix/src/px4_daemon/pxh.h rename to platforms/posix/src/px4/common/px4_daemon/pxh.h diff --git a/platforms/posix/src/px4_daemon/server.cpp b/platforms/posix/src/px4/common/px4_daemon/server.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/server.cpp rename to platforms/posix/src/px4/common/px4_daemon/server.cpp diff --git a/platforms/posix/src/px4_daemon/server.h b/platforms/posix/src/px4/common/px4_daemon/server.h similarity index 100% rename from platforms/posix/src/px4_daemon/server.h rename to platforms/posix/src/px4/common/px4_daemon/server.h diff --git a/platforms/posix/src/px4_daemon/server_io.cpp b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/server_io.cpp rename to platforms/posix/src/px4/common/px4_daemon/server_io.cpp diff --git a/platforms/posix/src/px4_daemon/sock_protocol.cpp b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp similarity index 100% rename from platforms/posix/src/px4_daemon/sock_protocol.cpp rename to platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp diff --git a/platforms/posix/src/px4_daemon/sock_protocol.h b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.h similarity index 100% rename from platforms/posix/src/px4_daemon/sock_protocol.h rename to platforms/posix/src/px4/common/px4_daemon/sock_protocol.h diff --git a/platforms/qurt/src/px4_layer/px4_init.cpp b/platforms/posix/src/px4/common/px4_init.cpp similarity index 96% rename from platforms/qurt/src/px4_layer/px4_init.cpp rename to platforms/posix/src/px4/common/px4_init.cpp index e162ed15cd1b..73387de35f0e 100644 --- a/platforms/qurt/src/px4_layer/px4_init.cpp +++ b/platforms/posix/src/px4/common/px4_init.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include int px4_platform_init(void) { diff --git a/platforms/posix/src/px4_layer/px4_posix_impl.cpp b/platforms/posix/src/px4/common/px4_posix_impl.cpp similarity index 97% rename from platforms/posix/src/px4_layer/px4_posix_impl.cpp rename to platforms/posix/src/px4/common/px4_posix_impl.cpp index 9c1cf6504214..3b31c6169869 100644 --- a/platforms/posix/src/px4_layer/px4_posix_impl.cpp +++ b/platforms/posix/src/px4/common/px4_posix_impl.cpp @@ -38,7 +38,6 @@ */ #include -#include #include #include #include @@ -98,10 +97,5 @@ void init(int argc, char *argv[], const char *app_name) #endif } -uint64_t get_time_micros() -{ - return hrt_absolute_time(); -} - } diff --git a/platforms/posix/src/px4_layer/px4_posix_tasks.cpp b/platforms/posix/src/px4/common/px4_posix_tasks.cpp similarity index 100% rename from platforms/posix/src/px4_layer/px4_posix_tasks.cpp rename to platforms/posix/src/px4/common/px4_posix_tasks.cpp diff --git a/platforms/posix/src/px4_layer/px4_sem.cpp b/platforms/posix/src/px4/common/px4_sem.cpp similarity index 99% rename from platforms/posix/src/px4_layer/px4_sem.cpp rename to platforms/posix/src/px4/common/px4_sem.cpp index e579c9222f64..d303f8e66ce1 100644 --- a/platforms/posix/src/px4_layer/px4_sem.cpp +++ b/platforms/posix/src/px4/common/px4_sem.cpp @@ -38,7 +38,6 @@ */ #include -#include #include #include #include diff --git a/platforms/posix/src/px4_layer/shmem_posix.cpp b/platforms/posix/src/px4/common/shmem_posix.cpp similarity index 100% rename from platforms/posix/src/px4_layer/shmem_posix.cpp rename to platforms/posix/src/px4/common/shmem_posix.cpp diff --git a/platforms/posix/src/CMakeLists.txt b/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt similarity index 88% rename from platforms/posix/src/CMakeLists.txt rename to platforms/posix/src/px4/common/test_stubs/CMakeLists.txt index c46e436e8e7b..1950cb8c8a75 100644 --- a/platforms/posix/src/CMakeLists.txt +++ b/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,11 @@ # ############################################################################ +set(SRCS + stub_daemon.cpp + stub_devmgr.cpp + stub_parameter.cpp + ) -add_subdirectory(px4_daemon) -add_subdirectory(px4_layer) - -add_subdirectory(lockstep_scheduler) +px4_add_library(test_stubs ${SRCS}) +message("-- ADDING TEST STUBS") diff --git a/platforms/posix/src/px4/common/test_stubs/stub_daemon.cpp b/platforms/posix/src/px4/common/test_stubs/stub_daemon.cpp new file mode 100644 index 000000000000..d379f90d5e2d --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_daemon.cpp @@ -0,0 +1,6 @@ +#include "stub_daemon.h" + + +void init_app_map(apps_map_type &apps) {stub_init_app_map_callback(apps);} + +void list_builtins(apps_map_type &apps) {stub_list_builtins_callback(apps);} diff --git a/platforms/posix/src/px4/common/test_stubs/stub_daemon.h b/platforms/posix/src/px4/common/test_stubs/stub_daemon.h new file mode 100644 index 000000000000..34119b177d26 --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_daemon.h @@ -0,0 +1,8 @@ +#pragma once + +#include + +#include + +std::function stub_init_app_map_callback = [](apps_map_type &) {}; +std::function stub_list_builtins_callback = [](apps_map_type &) {}; diff --git a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp new file mode 100644 index 000000000000..374712f7ba7d --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp @@ -0,0 +1,10 @@ +#include "stub_devmgr.h" + + +namespace DriverFramework +{ +int DevMgr::getNextDeviceName(unsigned int &index, const char **instancename) +{ + return stub_getNextDeviceName_callback(index, instancename); +} +} diff --git a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h new file mode 100644 index 000000000000..15840c889fce --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h @@ -0,0 +1,8 @@ +#pragma once + +#include "DevMgr.hpp" + +#include + +std::function stub_getNextDeviceName_callback = [](unsigned int &, +const char **) {return 0;}; diff --git a/platforms/posix/src/px4/common/test_stubs/stub_parameter.cpp b/platforms/posix/src/px4/common/test_stubs/stub_parameter.cpp new file mode 100644 index 000000000000..769d8d801086 --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_parameter.cpp @@ -0,0 +1,11 @@ +#include "stub_parameter.h" + + + +extern "C" { + /* This function blocks forever in tests, so override it with a version that can be customized */ + int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) + { + return stub_pthread_cond_wait_callback(cond, mutex); + } +} diff --git a/platforms/posix/src/px4/common/test_stubs/stub_parameter.h b/platforms/posix/src/px4/common/test_stubs/stub_parameter.h new file mode 100644 index 000000000000..57a709b54796 --- /dev/null +++ b/platforms/posix/src/px4/common/test_stubs/stub_parameter.h @@ -0,0 +1,8 @@ +#pragma once + +#include + +#include + +std::function stub_pthread_cond_wait_callback = +[](pthread_cond_t *, pthread_mutex_t *) {return 0;}; diff --git a/platforms/posix/src/px4/generic/CMakeLists.txt b/platforms/posix/src/px4/generic/CMakeLists.txt new file mode 100644 index 000000000000..ec489bf559c3 --- /dev/null +++ b/platforms/posix/src/px4/generic/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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/CMakeLists.txt b/platforms/posix/src/px4/generic/generic/CMakeLists.txt similarity index 94% rename from platforms/qurt/src/CMakeLists.txt rename to platforms/posix/src/px4/generic/generic/CMakeLists.txt index 602b1b710744..4db79a429f8e 100644 --- a/platforms/qurt/src/CMakeLists.txt +++ b/platforms/posix/src/px4/generic/generic/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,4 +31,6 @@ # ############################################################################ -add_subdirectory(px4_layer) + +add_subdirectory(tone_alarm) + diff --git a/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h b/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..d09b73c687dd --- /dev/null +++ b/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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/src/drivers/stm32/tone_alarm/CMakeLists.txt b/platforms/posix/src/px4/generic/generic/tone_alarm/CMakeLists.txt similarity index 97% rename from src/drivers/stm32/tone_alarm/CMakeLists.txt rename to platforms/posix/src/px4/generic/generic/tone_alarm/CMakeLists.txt index 5251d3910a7a..a2d2f8144bcc 100644 --- a/src/drivers/stm32/tone_alarm/CMakeLists.txt +++ b/platforms/posix/src/px4/generic/generic/tone_alarm/CMakeLists.txt @@ -31,6 +31,6 @@ # ############################################################################ -px4_add_library(tone_alarm_interface +px4_add_library(arch_tone_alarm ToneAlarmInterface.cpp ) diff --git a/src/drivers/sim/tone_alarm/ToneAlarmInterface.cpp b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp similarity index 91% rename from src/drivers/sim/tone_alarm/ToneAlarmInterface.cpp rename to platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp index 7e25fc774f73..c56fb8c45feb 100644 --- a/src/drivers/sim/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp @@ -35,20 +35,26 @@ * @file ToneAlarmInterface.cpp */ -#include +#include #include -void ToneAlarmInterface::init() +namespace ToneAlarmInterface +{ + +void init() { // Nothing to be done in simulation. } -void ToneAlarmInterface::start_note(unsigned frequency) +void start_note(unsigned frequency) { // Nothing to be done in simulation. } -void ToneAlarmInterface::stop_note() +void stop_note() { // Nothing to be done in simulation. } + +} /* namespace ToneAlarmInterface */ + diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index 3c17462dd7cc..1885d1078f3c 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -5,8 +5,6 @@ include(fastrpc) include(qurt_lib) include(qurt_flags) -add_subdirectory(src) - include_directories(${CMAKE_CURRENT_BINARY_DIR}) get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake index 18ecb396f943..94cdb61c23ec 100644 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ b/platforms/qurt/cmake/px4_impl_os.cmake @@ -45,6 +45,7 @@ # Required OS Inteface Functions # # * px4_os_add_flags +# * px4_os_determine_build_chip # * px4_os_prebuild_targets # @@ -94,8 +95,8 @@ function(px4_qurt_generate_builtin_commands) math(EXPR command_count "${command_count}+1") endif() endforeach() - configure_file(${PX4_SOURCE_DIR}/src/platforms/apps.cpp.in ${OUT}.cpp) - configure_file(${PX4_SOURCE_DIR}/src/platforms/apps.h.in ${OUT}.h) + 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() #============================================================================= @@ -144,6 +145,23 @@ function(px4_os_add_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 diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt new file mode 100644 index 000000000000..929788b587e2 --- /dev/null +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# 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_layer/CMakeLists.txt b/platforms/qurt/src/px4/common/CMakeLists.txt similarity index 96% rename from platforms/qurt/src/px4_layer/CMakeLists.txt rename to platforms/qurt/src/px4/common/CMakeLists.txt index 892a6e668474..1e168c411015 100644 --- a/platforms/qurt/src/px4_layer/CMakeLists.txt +++ b/platforms/qurt/src/px4/common/CMakeLists.txt @@ -40,7 +40,7 @@ set(QURT_LAYER_SRCS px4_qurt_impl.cpp px4_qurt_tasks.cpp lib_crc32.c - ../../../posix/src/px4_layer/drv_hrt.cpp + ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/drv_hrt.cpp qurt_stubs.c main.cpp shmem_qurt.cpp @@ -49,8 +49,8 @@ set(QURT_LAYER_SRCS if ("${QURT_ENABLE_STUBS}" STREQUAL "1") list(APPEND QURT_LAYER_SRCS - ../stubs/stubs_posix.c - ../stubs/stubs_qurt.c + stubs/posix.c + stubs/qurt.c ) endif() diff --git a/platforms/qurt/src/px4_layer/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c similarity index 100% rename from platforms/qurt/src/px4_layer/commands_hil.c rename to platforms/qurt/src/px4/common/commands_hil.c diff --git a/platforms/qurt/src/px4_layer/get_commands.h b/platforms/qurt/src/px4/common/get_commands.h similarity index 100% rename from platforms/qurt/src/px4_layer/get_commands.h rename to platforms/qurt/src/px4/common/get_commands.h 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 new file mode 100644 index 000000000000..d09b73c687dd --- /dev/null +++ b/platforms/qurt/src/px4/common/include/px4_platform/micro_hal.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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_layer/lib_crc32.c b/platforms/qurt/src/px4/common/lib_crc32.c similarity index 100% rename from platforms/qurt/src/px4_layer/lib_crc32.c rename to platforms/qurt/src/px4/common/lib_crc32.c diff --git a/platforms/qurt/src/px4_layer/main.cpp b/platforms/qurt/src/px4/common/main.cpp similarity index 99% rename from platforms/qurt/src/px4_layer/main.cpp rename to platforms/qurt/src/px4/common/main.cpp index 2fe6b57aac50..8f2aac9323ea 100644 --- a/platforms/qurt/src/px4_layer/main.cpp +++ b/platforms/qurt/src/px4/common/main.cpp @@ -38,10 +38,10 @@ * @author Julian Oes */ -#include #include #include #include +#include #include #include #include diff --git a/platforms/posix/src/px4_layer/px4_init.cpp b/platforms/qurt/src/px4/common/px4_init.cpp similarity index 96% rename from platforms/posix/src/px4_layer/px4_init.cpp rename to platforms/qurt/src/px4/common/px4_init.cpp index e162ed15cd1b..73387de35f0e 100644 --- a/platforms/posix/src/px4_layer/px4_init.cpp +++ b/platforms/qurt/src/px4/common/px4_init.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include int px4_platform_init(void) { diff --git a/platforms/qurt/src/px4_layer/px4_qurt_impl.cpp b/platforms/qurt/src/px4/common/px4_qurt_impl.cpp similarity index 99% rename from platforms/qurt/src/px4_layer/px4_qurt_impl.cpp rename to platforms/qurt/src/px4/common/px4_qurt_impl.cpp index 699f0c9ee506..41e5c6556690 100644 --- a/platforms/qurt/src/px4_layer/px4_qurt_impl.cpp +++ b/platforms/qurt/src/px4/common/px4_qurt_impl.cpp @@ -39,7 +39,6 @@ #include #include -#include #include #include #include diff --git a/platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp b/platforms/qurt/src/px4/common/px4_qurt_tasks.cpp similarity index 100% rename from platforms/qurt/src/px4_layer/px4_qurt_tasks.cpp rename to platforms/qurt/src/px4/common/px4_qurt_tasks.cpp diff --git a/platforms/qurt/src/px4_layer/qurt_stubs.c b/platforms/qurt/src/px4/common/qurt_stubs.c similarity index 100% rename from platforms/qurt/src/px4_layer/qurt_stubs.c rename to platforms/qurt/src/px4/common/qurt_stubs.c diff --git a/platforms/qurt/src/px4_layer/shmem_qurt.cpp b/platforms/qurt/src/px4/common/shmem_qurt.cpp similarity index 100% rename from platforms/qurt/src/px4_layer/shmem_qurt.cpp rename to platforms/qurt/src/px4/common/shmem_qurt.cpp diff --git a/src/platforms/qurt/stubs/stubs_posix.c b/platforms/qurt/src/px4/common/stubs/posix.c similarity index 100% rename from src/platforms/qurt/stubs/stubs_posix.c rename to platforms/qurt/src/px4/common/stubs/posix.c diff --git a/src/platforms/qurt/stubs/stubs_qurt.c b/platforms/qurt/src/px4/common/stubs/qurt.c similarity index 100% rename from src/platforms/qurt/stubs/stubs_qurt.c rename to platforms/qurt/src/px4/common/stubs/qurt.c diff --git a/platforms/qurt/src/px4/generic/CMakeLists.txt b/platforms/qurt/src/px4/generic/CMakeLists.txt new file mode 100644 index 000000000000..ec489bf559c3 --- /dev/null +++ b/platforms/qurt/src/px4/generic/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# 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/nuttx/src/CMakeLists.txt b/platforms/qurt/src/px4/generic/generic/CMakeLists.txt similarity index 94% rename from platforms/nuttx/src/CMakeLists.txt rename to platforms/qurt/src/px4/generic/generic/CMakeLists.txt index 602b1b710744..c4e98943e42e 100644 --- a/platforms/nuttx/src/CMakeLists.txt +++ b/platforms/qurt/src/px4/generic/generic/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,4 +31,5 @@ # ############################################################################ -add_subdirectory(px4_layer) + + 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 new file mode 100644 index 000000000000..d09b73c687dd --- /dev/null +++ b/platforms/qurt/src/px4/generic/generic/include/px4_arch/micro_hal.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * 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/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 8c6d76b61107..6c742afd076c 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -29,7 +29,7 @@ mc_pos_control start mc_att_control start fw_pos_control_l1 start fw_att_control start -wind_estimator start +airspeed_selector start #mixer load /dev/pwm_output0 ROMFS/sitl/mixers/standard_vtol_sitl.main.mix @@ -56,7 +56,7 @@ mc_pos_control status mc_att_control status fw_pos_control_l1 status fw_att_control status -wind_estimator status +airspeed_selector status dataman status uorb status @@ -72,7 +72,7 @@ navigator stop commander stop land_detector stop ekf2 stop -wind_estimator stop +airspeed_selector stop sensors stop sleep 2 diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config new file mode 100644 index 000000000000..60b34085086a --- /dev/null +++ b/posix-configs/rpi/px4_test.config @@ -0,0 +1,81 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +# navio config for a quad +uorb start +if [ -f eeprom/parameters ] +then + param load +fi +param set SYS_AUTOSTART 4001 +param set MAV_BROADCAST 1 +param set MAV_TYPE 2 +param set BAT_CNT_V_VOLT 0.001 +param set BAT_V_DIV 10.9176300578 +param set BAT_CNT_V_CURR 0.001 +param set BAT_A_PER_V 15.391030303 +dataman start +df_lsm9ds1_wrapper start -R 4 +#df_mpu9250_wrapper start -R 10 +#df_hmc5883_wrapper start +df_ms5611_wrapper start +navio_rgbled start +navio_adc start +gps start -d /dev/spidev0.0 -i spi -p ubx +sensors start +commander start +navigator start +ekf2 start +land_detector start multicopter +mc_pos_control start +mc_att_control start +mavlink start -x -u 14556 -r 1000000 +mavlink stream -u 14556 -s HIGHRES_IMU -r 50 +mavlink stream -u 14556 -s ATTITUDE -r 50 + +if [ -f /dev/ttyUSB0 ] +then + mavlink start -x -d /dev/ttyUSB0 + mavlink stream -d /dev/ttyUSB0 -s HIGHRES_IMU -r 50 + mavlink stream -d /dev/ttyUSB0 -s ATTITUDE -r 50 +fi + +navio_sysfs_rc_in start +linux_pwm_out start +logger start -t -b 200 +mavlink boot_complete + +sleep 5 + +ver all +sleep 1 +commander check +sleep 1 +dataman status +sleep 1 +ekf2 status +sleep 1 +logger status +sleep 1 +mavlink status +sleep 1 +mavlink status streams +sleep 1 +param show +sleep 1 +param status +sleep 1 +pwm info +sleep 1 +sensors status +sleep 1 +perf +sleep 1 +perf latency +sleep 1 +uorb top -1 +sleep 1 + +shutdown diff --git a/src/drivers/kinetis/adc/CMakeLists.txt b/src/drivers/adc/CMakeLists.txt similarity index 95% rename from src/drivers/kinetis/adc/CMakeLists.txt rename to src/drivers/adc/CMakeLists.txt index bf0eca22c49f..5e52fbf6dfe7 100644 --- a/src/drivers/kinetis/adc/CMakeLists.txt +++ b/src/drivers/adc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -36,4 +36,6 @@ px4_add_module( MAIN adc SRCS adc.cpp + DEPENDS + arch_adc ) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/adc/adc.cpp similarity index 53% rename from src/drivers/stm32/adc/adc.cpp rename to src/drivers/adc/adc.cpp index 7846d9bdbfe9..768d1034bd77 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/adc/adc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -34,92 +34,36 @@ /** * @file adc.cpp * - * Driver for the STM32 ADC. + * Driver for an ADC. * - * This is a low-rate driver, designed for sampling things like voltages - * and so forth. It avoids the gross complexity of the NuttX ADC driver. */ -#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 +using namespace time_literals; -#include -#include +#ifndef ADC_CHANNELS +#error "board needs to define ADC_CHANNELS to use this driver" +#endif -#if defined(ADC_CHANNELS) +#define ADC_TOTAL_CHANNELS 32 -/* - * Register accessors. - * For now, no reason not to just use ADC1. - */ -#define REG(_reg) (*(volatile uint32_t *)(STM32_ADC1_BASE + _reg)) - -#define rSR REG(STM32_ADC_SR_OFFSET) -#define rCR1 REG(STM32_ADC_CR1_OFFSET) -#define rCR2 REG(STM32_ADC_CR2_OFFSET) -#define rSMPR1 REG(STM32_ADC_SMPR1_OFFSET) -#define rSMPR2 REG(STM32_ADC_SMPR2_OFFSET) -#define rJOFR1 REG(STM32_ADC_JOFR1_OFFSET) -#define rJOFR2 REG(STM32_ADC_JOFR2_OFFSET) -#define rJOFR3 REG(STM32_ADC_JOFR3_OFFSET) -#define rJOFR4 REG(STM32_ADC_JOFR4_OFFSET) -#define rHTR REG(STM32_ADC_HTR_OFFSET) -#define rLTR REG(STM32_ADC_LTR_OFFSET) -#define rSQR1 REG(STM32_ADC_SQR1_OFFSET) -#define rSQR2 REG(STM32_ADC_SQR2_OFFSET) -#define rSQR3 REG(STM32_ADC_SQR3_OFFSET) -#define rJSQR REG(STM32_ADC_JSQR_OFFSET) -#define rJDR1 REG(STM32_ADC_JDR1_OFFSET) -#define rJDR2 REG(STM32_ADC_JDR2_OFFSET) -#define rJDR3 REG(STM32_ADC_JDR3_OFFSET) -#define rJDR4 REG(STM32_ADC_JDR4_OFFSET) -#define rDR REG(STM32_ADC_DR_OFFSET) - -#ifdef STM32_ADC_CCR -# define rCCR REG(STM32_ADC_CCR_OFFSET) - -/* Assuming VDC 2.4 - 3.6 */ - -#define ADC_MAX_FADC 36000000 - -# if STM32_PCLK2_FREQUENCY/2 <= ADC_MAX_FADC -# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV2 -# elif STM32_PCLK2_FREQUENCY/4 <= ADC_MAX_FADC -# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV4 -# elif STM32_PCLK2_FREQUENCY/6 <= ADC_MAX_FADC -# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV6 -# elif STM32_PCLK2_FREQUENCY/8 <= ADC_MAX_FADC -# define ADC_CCR_ADCPRE_DIV ADC_CCR_ADCPRE_DIV8 -# else -# error "ADC PCLK2 too high - no divisor found " -# endif -#endif -class ADC : public cdev::CDev +class ADC : public cdev::CDev, public px4::ScheduledWorkItem { public: - ADC(uint32_t channels); + ADC(uint32_t base_address, uint32_t channels); ~ADC(); virtual int init(); @@ -132,51 +76,43 @@ class ADC : public cdev::CDev virtual int close_last(struct file *filp); private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; - perf_counter_t _sample_perf; - - unsigned _channel_count; - px4_adc_msg_t *_samples; /**< sample buffer */ - - orb_advert_t _to_system_power; - orb_advert_t _to_adc_report; - - /** work trampoline */ - static void _tick_trampoline(void *arg); - - /** worker function */ - void _tick(); + void Run() override; /** * Sample a single channel and return the measured value. * * @param channel The channel to sample. - * @return The sampled value, or 0xffff if - * sampling failed. + * @return The sampled value, or 0xffff if sampling failed. */ - uint16_t _sample(unsigned channel); + uint16_t sample(unsigned channel); + + void update_adc_report(hrt_abstime now); + void update_system_power(hrt_abstime now); - // update system_power ORB topic, only on FMUv2 - void update_system_power(hrt_abstime now); - void update_adc_report(hrt_abstime now); + static const hrt_abstime kINTERVAL{10_ms}; /**< 100Hz base rate */ + + perf_counter_t _sample_perf; + + unsigned _channel_count{0}; + const uint32_t _base_address; + px4_adc_msg_t *_samples{nullptr}; /**< sample buffer */ + + uORB::Publication _to_adc_report{ORB_ID(adc_report)}; + uORB::Publication _to_system_power{ORB_ID(system_power)}; }; -ADC::ADC(uint32_t channels) : +ADC::ADC(uint32_t base_address, uint32_t channels) : CDev(ADC0_DEVICE_PATH), + ScheduledWorkItem(px4::wq_configurations::hp_default), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), - _channel_count(0), - _samples(nullptr), - _to_system_power(nullptr), - _to_adc_report(nullptr) + _base_address(base_address) { /* always enable the temperature sensor */ - channels |= 1 << 16; + channels |= px4_arch_adc_temp_sensor_mask(); /* allocate the sample array */ - for (unsigned i = 0; i < 32; i++) { + for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { if (channels & (1 << i)) { _channel_count++; } @@ -192,7 +128,7 @@ ADC::ADC(uint32_t channels) : if (_samples != nullptr) { unsigned index = 0; - for (unsigned i = 0; i < 32; i++) { + for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; _samples[index].am_data = 0; @@ -207,85 +143,15 @@ ADC::~ADC() if (_samples != nullptr) { delete _samples; } -} - -int board_adc_init() -{ - static bool once = false; - - if (!once) { - - once = true; - - /* do calibration if supported */ -#ifdef ADC_CR2_CAL - rCR2 |= ADC_CR2_CAL; - px4_usleep(100); - - if (rCR2 & ADC_CR2_CAL) { - return -1; - } - -#endif - - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; - rSMPR2 = 0b00011011011011011011011011011011; - - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - - /* Soc have CCR */ -#ifdef STM32_ADC_CCR -# ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE | ADC_CCR_ADCPRE_DIV; -# else - rCCR = ADC_CCR_ADCPRE_DIV; -# endif -#endif - /* configure for a single-channel sequence */ - rSQR1 = 0; - rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - px4_usleep(10); - rCR2 |= ADC_CR2_ADON; - px4_usleep(10); - rCR2 |= ADC_CR2_ADON; - px4_usleep(10); - - /* kick off a sample and wait for it to complete */ - hrt_abstime now = hrt_absolute_time(); - rCR2 |= ADC_CR2_SWSTART; - - while (!(rSR & ADC_SR_EOC)) { - - /* 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; - } - } - } // once - - return OK; + perf_free(_sample_perf); + px4_arch_adc_uninit(_base_address); } int ADC::init() { - int rv = board_adc_init(); + int rv = px4_arch_adc_init(_base_address); if (rv < 0) { PX4_DEBUG("sample timeout"); @@ -323,10 +189,10 @@ int ADC::open_first(struct file *filp) { /* get fresh data */ - _tick(); + Run(); /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); + ScheduleOnInterval(kINTERVAL, kINTERVAL); return 0; } @@ -334,24 +200,19 @@ ADC::open_first(struct file *filp) int ADC::close_last(struct file *filp) { - hrt_cancel(&_call); - return 0; -} + ScheduleClear(); -void -ADC::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); + return 0; } void -ADC::_tick() +ADC::Run() { hrt_abstime now = hrt_absolute_time(); /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) { - _samples[i].am_data = _sample(_samples[i].am_channel); + _samples[i].am_data = sample(_samples[i].am_channel); } update_adc_report(now); @@ -375,21 +236,16 @@ ADC::update_adc_report(hrt_abstime now) adc.channel_value[i] = _samples[i].am_data * 3.3f / 4096.0f; } - int instance; - orb_publish_auto(ORB_ID(adc_report), &_to_adc_report, &adc, &instance, ORB_PRIO_HIGH); + _to_adc_report.publish(adc); } void ADC::update_system_power(hrt_abstime now) { #if defined (BOARD_ADC_USB_CONNECTED) - system_power_s system_power = {}; + system_power_s system_power {}; system_power.timestamp = now; - system_power.voltage5v_v = 0; - system_power.voltage3v3_v = 0; - system_power.v3v3_valid = 0; - /* Assume HW provides only ADC_SCALED_V5_SENSE */ int cnt = 1; /* HW provides both ADC_SCALED_V5_SENSE and ADC_SCALED_V3V3_SENSORS_SENSE */ @@ -455,59 +311,23 @@ ADC::update_system_power(hrt_abstime now) #ifdef BOARD_ADC_PERIPH_5V_OC // OC pins are active low system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC; -#else - system_power.periph_5v_oc = 0; #endif + #ifdef BOARD_ADC_HIPOWER_5V_OC system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; -#else - system_power.hipower_5v_oc = 0; #endif /* lazily publish */ - if (_to_system_power != nullptr) { - orb_publish(ORB_ID(system_power), _to_system_power, &system_power); - - } else { - _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); - } + _to_system_power.publish(system_power); #endif // BOARD_ADC_USB_CONNECTED } -uint16_t board_adc_sample(unsigned channel) -{ - /* clear any previous EOC */ - - if (rSR & ADC_SR_EOC) { - rSR &= ~ADC_SR_EOC; - } - - /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ - rSQR3 = channel; - rCR2 |= ADC_CR2_SWSTART; - - /* wait for the conversion to complete */ - hrt_abstime now = hrt_absolute_time(); - - while (!(rSR & ADC_SR_EOC)) { - - /* 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) { - return 0xffff; - } - } - - /* read the result and clear EOC */ - uint16_t result = rDR; - return result; -} - uint16_t -ADC::_sample(unsigned channel) +ADC::sample(unsigned channel) { perf_begin(_sample_perf); - uint16_t result = board_adc_sample(channel); + uint16_t result = px4_arch_adc_sample(_base_address, channel); if (result == 0xffff) { PX4_ERR("sample timeout"); @@ -524,24 +344,26 @@ extern "C" __EXPORT int adc_main(int argc, char *argv[]); namespace { -ADC *g_adc; +ADC *g_adc{nullptr}; -void +int test(void) { int fd = open(ADC0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - err(1, "can't open ADC device"); + PX4_ERR("can't open ADC device %d", errno); + return 1; } for (unsigned i = 0; i < 50; i++) { - px4_adc_msg_t data[PX4_MAX_ADC_CHANNELS]; + px4_adc_msg_t data[ADC_TOTAL_CHANNELS]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) { - errx(1, "read error"); + PX4_ERR("read error"); + return 1; } unsigned channels = count / sizeof(data[0]); @@ -554,7 +376,7 @@ test(void) px4_usleep(500000); } - exit(0); + return 0; } } @@ -562,25 +384,25 @@ int adc_main(int argc, char *argv[]) { if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ - g_adc = new ADC(ADC_CHANNELS); + g_adc = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS); if (g_adc == nullptr) { - errx(1, "couldn't allocate the ADC driver"); + PX4_ERR("couldn't allocate the ADC driver"); + return 1; } if (g_adc->init() != OK) { delete g_adc; - errx(1, "ADC init failed"); + PX4_ERR("ADC init failed"); + return 1; } } if (argc > 1) { if (!strcmp(argv[1], "test")) { - test(); + return test(); } } - exit(0); + return 0; } -#endif diff --git a/src/drivers/barometer/bmp280/CMakeLists.txt b/src/drivers/barometer/bmp280/CMakeLists.txt index 9d4544eb57c3..27ee0e615b30 100644 --- a/src/drivers/barometer/bmp280/CMakeLists.txt +++ b/src/drivers/barometer/bmp280/CMakeLists.txt @@ -35,9 +35,7 @@ px4_add_module( MODULE drivers__bmp280 MAIN bmp280 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable - STACK_MAIN - 1200 + -Wno-cast-align # TODO: fix and enable SRCS bmp280_spi.cpp bmp280_i2c.cpp diff --git a/src/drivers/barometer/bmp280/bmp280.h b/src/drivers/barometer/bmp280/bmp280.h index 5f81383eb15f..4b50887a7315 100644 --- a/src/drivers/barometer/bmp280/bmp280.h +++ b/src/drivers/barometer/bmp280/bmp280.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include "board_config.h" diff --git a/src/drivers/barometer/lps22hb/LPS22HB.hpp b/src/drivers/barometer/lps22hb/LPS22HB.hpp index 0eacf0877a6d..f30dbeded96a 100644 --- a/src/drivers/barometer/lps22hb/LPS22HB.hpp +++ b/src/drivers/barometer/lps22hb/LPS22HB.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/barometer/lps25h/CMakeLists.txt b/src/drivers/barometer/lps25h/CMakeLists.txt index 77619cf38da5..457f69aa2f96 100644 --- a/src/drivers/barometer/lps25h/CMakeLists.txt +++ b/src/drivers/barometer/lps25h/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__lps25h MAIN lps25h - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/barometer/lps25h/lps25h.h b/src/drivers/barometer/lps25h/lps25h.h index fadf2b34189a..bff2db2df212 100644 --- a/src/drivers/barometer/lps25h/lps25h.h +++ b/src/drivers/barometer/lps25h/lps25h.h @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/barometer/mpl3115a2/CMakeLists.txt b/src/drivers/barometer/mpl3115a2/CMakeLists.txt index 5d14aeb12404..74d452b9aea6 100644 --- a/src/drivers/barometer/mpl3115a2/CMakeLists.txt +++ b/src/drivers/barometer/mpl3115a2/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__mpl3115a2 MAIN mpl3115a2 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/barometer/mpl3115a2/mpl3115a2.h b/src/drivers/barometer/mpl3115a2/mpl3115a2.h index 0a6be7c00657..17de2e4612c9 100644 --- a/src/drivers/barometer/mpl3115a2/mpl3115a2.h +++ b/src/drivers/barometer/mpl3115a2/mpl3115a2.h @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include "board_config.h" diff --git a/src/drivers/barometer/ms5611/CMakeLists.txt b/src/drivers/barometer/ms5611/CMakeLists.txt index 8da539b0e1aa..24e2d1382afa 100644 --- a/src/drivers/barometer/ms5611/CMakeLists.txt +++ b/src/drivers/barometer/ms5611/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE drivers__ms5611 MAIN ms5611 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/barometer/ms5611/ms5611.h b/src/drivers/barometer/ms5611/ms5611.h index cc197dc666d9..6079872353cf 100644 --- a/src/drivers/barometer/ms5611/ms5611.h +++ b/src/drivers/barometer/ms5611/ms5611.h @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/barometer/ms5611/ms5611_spi.cpp b/src/drivers/barometer/ms5611/ms5611_spi.cpp index e0895360f6f8..343201f58ec0 100644 --- a/src/drivers/barometer/ms5611/ms5611_spi.cpp +++ b/src/drivers/barometer/ms5611/ms5611_spi.cpp @@ -124,14 +124,7 @@ MS5611_SPI::MS5611_SPI(uint8_t bus, uint32_t device, ms5611::prom_u &prom_buf) : int MS5611_SPI::init() { - int ret; - -#if defined(PX4_SPI_BUS_RAMTRON) && \ - (PX4_SPI_BUS_BARO == PX4_SPI_BUS_RAMTRON) - SPI::set_lockmode(LOCK_THREADS); -#endif - - ret = SPI::init(); + int ret = SPI::init(); if (ret != OK) { PX4_DEBUG("SPI init failed"); diff --git a/src/drivers/batt_smbus/CMakeLists.txt b/src/drivers/batt_smbus/CMakeLists.txt index 1c8ff57c1430..bf60e80af307 100644 --- a/src/drivers/batt_smbus/CMakeLists.txt +++ b/src/drivers/batt_smbus/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__batt_smbus MAIN batt_smbus - STACK_MAIN 1100 COMPILE_FLAGS SRCS batt_smbus.cpp diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h index ed5d864ec2e2..452fe59bb4e0 100644 --- a/src/drivers/batt_smbus/batt_smbus.h +++ b/src/drivers/batt_smbus/batt_smbus.h @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include "board_config.h" diff --git a/src/drivers/boards/common/CMakeLists.txt b/src/drivers/boards/common/CMakeLists.txt index d8f9b08239b8..14c165d84823 100644 --- a/src/drivers/boards/common/CMakeLists.txt +++ b/src/drivers/boards/common/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ # common board drivers (currently only for nuttx fmu boards) -if ((${PX4_PLATFORM} MATCHES "nuttx") AND NOT ${PX4_BOARD} MATCHES "io") +if ((${PX4_PLATFORM} MATCHES "nuttx") AND NOT ${PX4_BOARD} MATCHES "px4_io") add_library(drivers_boards_common board_crashdump.c diff --git a/src/drivers/boards/common/board_internal_common.h b/src/drivers/boards/common/board_internal_common.h index 47181b473f7f..f01eabdc8c7f 100644 --- a/src/drivers/boards/common/board_internal_common.h +++ b/src/drivers/boards/common/board_internal_common.h @@ -45,39 +45,6 @@ * Included Files ************************************************************************************/ -/************************************************************************************ - * Name: board_adc_init - * - * Description: - * boards may provide this function to allow complex version-ing. - * - * Input Parameters: - * None. - * - * Returned Value: - * - * OK, or -1 if the function failed. - */ - -__EXPORT int board_adc_init(void); - -/************************************************************************************ - * Name: board_adc_sample - * - * Description: - * boards provide this function to allow complex version-ing. - * - * Input Parameters: - * channel - The number of the adc channel to read. - * - * Returned Value: - * The ADC DN read for the channel or 0xffff if there - * is an error reading the channel. - */ - -__EXPORT uint16_t board_adc_sample(unsigned channel); - - /************************************************************************************ * Name: board_gpio_init * diff --git a/src/drivers/boards/common/stm32/board_hw_rev_ver.c b/src/drivers/boards/common/stm32/board_hw_rev_ver.c index bd59bd7c76d2..d3e83df51a6a 100644 --- a/src/drivers/boards/common/stm32/board_hw_rev_ver.c +++ b/src/drivers/boards/common/stm32/board_hw_rev_ver.c @@ -37,6 +37,8 @@ * Implementation of STM32 based Board Hardware Revision and Version ID API */ +#include +#include #include #include #include "board_config.h" @@ -199,11 +201,11 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc /* Yes - Fire up the ADC (it has once control) */ - if (board_adc_init() == OK) { + if (px4_arch_adc_init(HW_REV_VER_ADC_BASE) == OK) { /* Read the value */ for (unsigned av = 0; av < samples; av++) { - dn = board_adc_sample(adc_channel); + dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel); if (dn == 0xffff) { break; diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 074673cd6834..f731110bffa6 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -45,25 +45,12 @@ namespace camera_capture { -CameraCapture *g_camera_capture; +CameraCapture *g_camera_capture{nullptr}; } CameraCapture::CameraCapture() : - _capture_enabled(false), - _gpio_capture(false), - _trigger_pub(nullptr), - _command_ack_pub(nullptr), - _command_sub(-1), - _trig_buffer(nullptr), - _camera_capture_mode(0), - _camera_capture_edge(0), - _capture_seq(0), - _last_trig_begin_time(0), - _last_exposure_time(0), - _capture_overflows(0) + ScheduledWorkItem(px4::wq_configurations::lp_default) { - - memset(&_work, 0, sizeof(_work)); memset(&_work_publisher, 0, sizeof(_work_publisher)); // Capture Parameters @@ -75,7 +62,6 @@ CameraCapture::CameraCapture() : _p_camera_capture_edge = param_find("CAM_CAP_EDGE"); param_get(_p_camera_capture_edge, &_camera_capture_edge); - } CameraCapture::~CameraCapture() @@ -89,10 +75,8 @@ CameraCapture::~CameraCapture() } void -CameraCapture::capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +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.edge_state = edge_state; @@ -114,7 +98,6 @@ CameraCapture::gpio_interrupt_routine(int irq, void *context, void *arg) work_queue(HPWORK, &_work_publisher, (worker_t)&CameraCapture::publish_trigger_trampoline, dev, 0); return PX4_OK; - } void @@ -130,7 +113,7 @@ CameraCapture::publish_trigger() { bool publish = false; - struct camera_trigger_s trigger {}; + camera_trigger_s trigger{}; // MODES 1 and 2 are not fully tested if (_camera_capture_mode == 0 || _gpio_capture) { @@ -173,48 +156,23 @@ CameraCapture::publish_trigger() return; } - if (_trigger_pub == nullptr) { - - _trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger); - - } else { - - orb_publish(ORB_ID(camera_trigger), _trigger_pub, &trigger); - } - + _trigger_pub.publish(trigger); } void -CameraCapture::capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow) { camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); } void -CameraCapture::cycle_trampoline(void *arg) -{ - CameraCapture *dev = reinterpret_cast(arg); - - dev->cycle(); -} - -void -CameraCapture::cycle() +CameraCapture::Run() { - - if (_command_sub < 0) { - _command_sub = orb_subscribe(ORB_ID(vehicle_command)); - } - - bool updated = false; - orb_check(_command_sub, &updated); - // Command handling - if (updated) { + vehicle_command_s cmd{}; - vehicle_command_s cmd; - orb_copy(ORB_ID(vehicle_command), _command_sub, &cmd); + if (_command_sub.update(&cmd)) { // TODO : this should eventuallly be a capture control command if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) { @@ -235,31 +193,17 @@ CameraCapture::cycle() } // Acknowledge the command - vehicle_command_ack_s command_ack = { - .timestamp = 0, - .result_param2 = 0, - .command = cmd.command, - .result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, - .from_external = false, - .result_param1 = 0, - .target_system = cmd.source_system, - .target_component = cmd.source_component - }; - - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); + vehicle_command_ack_s command_ack{}; - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); + command_ack.timestamp = hrt_absolute_time(); + command_ack.command = cmd.command; + command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; - } + _command_ack_pub.publish(command_ack); } - } - - work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, camera_capture::g_camera_capture, - USEC2TICK(100000)); // 100ms } void @@ -267,9 +211,7 @@ CameraCapture::set_capture_control(bool enabled) { #if !defined CONFIG_ARCH_BOARD_AV_X_V1 - int fd = -1; - - fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); + int fd = ::open(PX4FMU_DEVICE_PATH, O_RDWR); if (fd < 0) { PX4_ERR("open fail"); @@ -343,7 +285,9 @@ CameraCapture::set_capture_control(bool enabled) void CameraCapture::reset_statistics(bool reset_seq) { - if (reset_seq) { _capture_seq = 0; } + if (reset_seq) { + _capture_seq = 0; + } _last_trig_begin_time = 0; _last_exposure_time = 0; @@ -361,9 +305,8 @@ CameraCapture::start() return PX4_ERROR; } - // start to monitor at low rates for capture control commands - work_queue(LPWORK, &_work, (worker_t)&CameraCapture::cycle_trampoline, this, - USEC2TICK(1)); + // run every 100 ms (10 Hz) + ScheduleOnInterval(100000, 10000); return PX4_OK; } @@ -371,8 +314,8 @@ CameraCapture::start() void CameraCapture::stop() { + ScheduleClear(); - work_cancel(LPWORK, &_work); work_cancel(HPWORK, &_work_publisher); if (camera_capture::g_camera_capture != nullptr) { diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp index 95fc1a41c03a..977ae47f6ab1 100644 --- a/src/drivers/camera_capture/camera_capture.hpp +++ b/src/drivers/camera_capture/camera_capture.hpp @@ -38,22 +38,23 @@ #pragma once -#include -#include - -#include -#include -#include - #include #include -#include #include - +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include #define PX4FMU_DEVICE_PATH "/dev/px4fmu" @@ -61,7 +62,7 @@ #define GPIO_TRIG_AVX /* PD14 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14) -class CameraCapture +class CameraCapture : public px4::ScheduledWorkItem { public: /** @@ -86,10 +87,10 @@ class CameraCapture void status(); - void cycle(); + // Low-rate command handling loop + void Run() override; - static void capture_trampoline(void *context, uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, + static void capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); void set_capture_control(bool enabled); @@ -99,20 +100,16 @@ class CameraCapture void publish_trigger(); - static struct work_s _work; static struct work_s _work_publisher; private: - bool _capture_enabled; - bool _gpio_capture; - // Publishers - orb_advert_t _trigger_pub; - orb_advert_t _command_ack_pub; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _trigger_pub{ORB_ID(camera_trigger)}; // Subscribers - int _command_sub; + uORB::Subscription _command_sub{ORB_ID(vehicle_command)}; // Trigger Buffer struct _trig_s { @@ -120,30 +117,30 @@ class CameraCapture hrt_abstime edge_time; uint32_t edge_state; uint32_t overflow; - }; + } _trigger{}; - struct _trig_s _trigger; + ringbuffer::RingBuffer *_trig_buffer{nullptr}; - ringbuffer::RingBuffer *_trig_buffer; + bool _capture_enabled{false}; + bool _gpio_capture{false}; // Parameters - param_t _p_strobe_delay; - float _strobe_delay; - param_t _p_camera_capture_mode; - int32_t _camera_capture_mode; - param_t _p_camera_capture_edge; - int32_t _camera_capture_edge; + param_t _p_strobe_delay{PARAM_INVALID}; + float _strobe_delay{0.0f}; + param_t _p_camera_capture_mode{PARAM_INVALID}; + int32_t _camera_capture_mode{0}; + param_t _p_camera_capture_edge{PARAM_INVALID}; + int32_t _camera_capture_edge{0}; // Signal capture statistics - uint32_t _capture_seq; - hrt_abstime _last_trig_begin_time; - hrt_abstime _last_exposure_time; - hrt_abstime _last_trig_time; - uint32_t _capture_overflows; + uint32_t _capture_seq{0}; + hrt_abstime _last_trig_begin_time{0}; + hrt_abstime _last_exposure_time{0}; + hrt_abstime _last_trig_time{0}; + uint32_t _capture_overflows{0}; // Signal capture callback - void capture_callback(uint32_t chan_index, - hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); // GPIO interrupt routine (for AV_X board) static int gpio_interrupt_routine(int irq, void *context, void *arg); @@ -151,9 +148,6 @@ class CameraCapture // Signal capture publish static void publish_trigger_trampoline(void *arg); - // Low-rate command handling loop - static void cycle_trampoline(void *arg); - }; -struct work_s CameraCapture::_work; + struct work_s CameraCapture::_work_publisher; diff --git a/src/drivers/camera_trigger/CMakeLists.txt b/src/drivers/camera_trigger/CMakeLists.txt index ff9bbe487bb0..efe00812b915 100644 --- a/src/drivers/camera_trigger/CMakeLists.txt +++ b/src/drivers/camera_trigger/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__camera_trigger MAIN camera_trigger - STACK_MAIN 1200 COMPILE_FLAGS SRCS camera_trigger.cpp diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index a5b118606921..7c2d93dd86d3 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -50,11 +50,12 @@ #include #include #include -#include +#include #include #include #include +#include #include #include #include @@ -132,7 +133,7 @@ class CameraTrigger : public px4::ScheduledWorkItem /** * Start the task. */ - void start(); + bool start(); /** * Stop the task. @@ -174,7 +175,8 @@ class CameraTrigger : public px4::ScheduledWorkItem uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; orb_advert_t _trigger_pub; - orb_advert_t _cmd_ack_pub; + + uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; param_t _p_mode; param_t _p_activation_time; @@ -192,7 +194,7 @@ class CameraTrigger : public px4::ScheduledWorkItem /** * Vehicle command handler */ - void Run(); + void Run() override; /** * Fires trigger @@ -250,7 +252,6 @@ CameraTrigger::CameraTrigger() : _last_shoot_position(0.0f, 0.0f), _valid_position(false), _trigger_pub(nullptr), - _cmd_ack_pub(nullptr), _trigger_mode(TRIGGER_MODE_NONE), _cam_cap_fback(0), _camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO), @@ -328,7 +329,9 @@ CameraTrigger::CameraTrigger() : CameraTrigger::~CameraTrigger() { - delete (_camera_interface); + if (_camera_interface != nullptr) { + delete (_camera_interface); + } camera_trigger::g_camera_trigger = nullptr; } @@ -426,9 +429,19 @@ CameraTrigger::shoot_once() } } -void +bool CameraTrigger::start() { + if (_camera_interface == nullptr) { + if (camera_trigger::g_camera_trigger != nullptr) { + delete (camera_trigger::g_camera_trigger); + camera_trigger::g_camera_trigger = nullptr; + + } + + return false; + } + if ((_trigger_mode == TRIGGER_MODE_INTERVAL_ALWAYS_ON || _trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) && _camera_interface->has_power_control() && @@ -455,6 +468,8 @@ CameraTrigger::start() // start to monitor at high rate for trigger enable command ScheduleNow(); + + return true; } void @@ -471,18 +486,20 @@ CameraTrigger::stop() if (camera_trigger::g_camera_trigger != nullptr) { delete (camera_trigger::g_camera_trigger); + camera_trigger::g_camera_trigger = nullptr; } } void CameraTrigger::test() { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.param5 = 1.0; vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; - orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); } void @@ -682,20 +699,15 @@ CameraTrigger::Run() // Command ACK handling if (updated && need_ack) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)cmd_result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (_cmd_ack_pub == nullptr) { - _cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _cmd_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); } ScheduleDelayed(poll_interval_usec); @@ -830,7 +842,11 @@ int camera_trigger_main(int argc, char *argv[]) return 1; } - camera_trigger::g_camera_trigger->start(); + if (!camera_trigger::g_camera_trigger->start()) { + PX4_WARN("failed to start camera trigger"); + return 1; + } + return 0; } diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 1d5e6e0d1cc6..35562222db29 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -112,8 +112,8 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); /** * Camera trigger pin * - * Selects which pin is used, ranges from 1 to 6 (AUX1-AUX6 on px4_fmu-v2 and the rail - * pins on px4_fmu-v4). The PWM interface takes two pins per camera, while relay + * Selects which FMU pin is used (range: AUX1-AUX6 on Pixhawk controllers with an I/O board, + * MAIN1-MAIN6 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. diff --git a/src/drivers/differential_pressure/ets/CMakeLists.txt b/src/drivers/differential_pressure/ets/CMakeLists.txt index cbe53b949018..fe77c1172c28 100644 --- a/src/drivers/differential_pressure/ets/CMakeLists.txt +++ b/src/drivers/differential_pressure/ets/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__ets_airspeed MAIN ets_airspeed - STACK_MAIN 1200 COMPILE_FLAGS SRCS ets_airspeed.cpp diff --git a/src/drivers/differential_pressure/ms4525/CMakeLists.txt b/src/drivers/differential_pressure/ms4525/CMakeLists.txt index bb7b5c847fbd..4173508249b3 100644 --- a/src/drivers/differential_pressure/ms4525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms4525/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__ms4525_airspeed MAIN ms4525_airspeed - STACK_MAIN 1200 COMPILE_FLAGS SRCS ms4525_airspeed.cpp diff --git a/src/drivers/differential_pressure/ms5525/CMakeLists.txt b/src/drivers/differential_pressure/ms5525/CMakeLists.txt index 1e90ef4b67de..7214d5937848 100644 --- a/src/drivers/differential_pressure/ms5525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms5525/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__ms5525_airspeed MAIN ms5525_airspeed - STACK_MAIN 1200 COMPILE_FLAGS SRCS MS5525.cpp diff --git a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt index bca086dbeabe..2c45466e727c 100644 --- a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt +++ b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__sdp3x_airspeed MAIN sdp3x_airspeed - STACK_MAIN 1200 COMPILE_FLAGS SRCS SDP3X.cpp diff --git a/src/drivers/distance_sensor/cm8jl65/CMakeLists.txt b/src/drivers/distance_sensor/cm8jl65/CMakeLists.txt index 3c4f667a71c0..e64c51a5e2c0 100644 --- a/src/drivers/distance_sensor/cm8jl65/CMakeLists.txt +++ b/src/drivers/distance_sensor/cm8jl65/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__cm8jl65 MAIN cm8jl65 - STACK_MAIN 1200 SRCS cm8jl65.cpp MODULE_CONFIG diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp index 07aa1c2866ea..5f6a88a1d801 100644 --- a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include @@ -227,8 +227,9 @@ class CM8JL65 : public cdev::CDev, public px4::ScheduledWorkItem uint16_t _crc16{0}; - float _max_distance{9.0f}; - float _min_distance{0.10f}; + // Use conservative distance bounds, to make sure we don't fuse garbage data + float _max_distance{7.9f}; // Datasheet: 8.0m + float _min_distance{0.2f}; // Datasheet: 0.17m CM8JL65_PARSE_STATE _parse_state{WAITING_FRAME}; @@ -335,7 +336,7 @@ CM8JL65::collect() report.max_distance = _max_distance; report.min_distance = _min_distance; report.orientation = _rotation; - report.signal_quality = -1; + report.signal_quality = report.current_distance < _max_distance && report.current_distance > _min_distance ? -1 : 0; report.timestamp = hrt_absolute_time(); report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; report.variance = 0.0f; diff --git a/src/drivers/distance_sensor/hc_sr04/CMakeLists.txt b/src/drivers/distance_sensor/hc_sr04/CMakeLists.txt index 2491ea42028a..fd32cc36a8c1 100644 --- a/src/drivers/distance_sensor/hc_sr04/CMakeLists.txt +++ b/src/drivers/distance_sensor/hc_sr04/CMakeLists.txt @@ -34,9 +34,8 @@ px4_add_module( MODULE drivers__hc_sr04 MAIN hc_sr04 - STACK_MAIN 1200 COMPILE_FLAGS SRCS hc_sr04.cpp DEPENDS - ) \ No newline at end of file + ) diff --git a/src/drivers/distance_sensor/leddar_one/CMakeLists.txt b/src/drivers/distance_sensor/leddar_one/CMakeLists.txt index 2ed26472c254..bbaf1e6ef1b0 100644 --- a/src/drivers/distance_sensor/leddar_one/CMakeLists.txt +++ b/src/drivers/distance_sensor/leddar_one/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__leddar_one MAIN leddar_one - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp index 5cfdf1a681b1..50ef8a8c2549 100644 --- a/src/drivers/distance_sensor/leddar_one/leddar_one.cpp +++ b/src/drivers/distance_sensor/leddar_one/leddar_one.cpp @@ -31,47 +31,40 @@ * ****************************************************************************/ -#include -#include -#include -#include -#include #include #include #include #include - #include #include #include - +#include #include - +#include +#include +#include +#include +#include #include using namespace time_literals; -#define DEVICE_PATH "/dev/LeddarOne" -#define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3" +#define DEVICE_PATH "/dev/LeddarOne" +#define LEDDAR_ONE_DEFAULT_SERIAL_PORT "/dev/ttyS3" -#define MAX_DISTANCE 40.0f -#define MIN_DISTANCE 0.01f +#define LEDDAR_ONE_FIELD_OF_VIEW (0.105f) // 6 deg cone angle. -#define SENSOR_READING_FREQ 10.0f -#define READING_USEC_PERIOD (unsigned long)(1000000.0f / SENSOR_READING_FREQ) -#define OVERSAMPLE 6 -#define WORK_USEC_INTERVAL READING_USEC_PERIOD / OVERSAMPLE -#define COLLECT_USEC_TIMEOUT READING_USEC_PERIOD / (OVERSAMPLE / 2) +#define LEDDAR_ONE_MAX_DISTANCE 40.0f +#define LEDDAR_ONE_MIN_DISTANCE 0.01f -/* 0.5sec */ -#define PROBE_USEC_TIMEOUT 500000_us +#define LEDDAR_ONE_MEASURE_INTERVAL 100_ms // 10Hz -#define MODBUS_SLAVE_ADDRESS 0x01 -#define MODBUS_READING_FUNCTION 0x04 -#define READING_START_ADDR 0x14 -#define READING_LEN 0xA +#define MODBUS_SLAVE_ADDRESS 0x01 +#define MODBUS_READING_FUNCTION 0x04 +#define READING_START_ADDR 0x14 +#define READING_LEN 0xA static const uint8_t request_reading_msg[] = { MODBUS_SLAVE_ADDRESS, @@ -114,7 +107,9 @@ struct __attribute__((__packed__)) reading_msg { class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem { public: - LeddarOne(const char *device_path, const char *serial_port, uint8_t rotation); + LeddarOne(const char *device_path, + const char *serial_port = LEDDAR_ONE_DEFAULT_SERIAL_PORT, + const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); virtual ~LeddarOne(); virtual int init() override; @@ -141,13 +136,16 @@ class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem * @param data_frame The data frame to compute a checksum for. * @param crc16_length The length of the data frame. */ - uint16_t crc16_calc(const unsigned char *data_frame, uint8_t crc16_length); + uint16_t crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length); /** * Reads the data measrurement from serial UART. */ int collect(); + /** + * Sends a data request message to the sensor. + */ int measure(); /** @@ -156,56 +154,48 @@ class LeddarOne : public cdev::CDev, public px4::ScheduledWorkItem */ int open_serial_port(const speed_t speed = B115200); - void publish(uint16_t distance_cm); - - bool request(); - void Run() override; - const char *_serial_port; + const char *_serial_port{nullptr}; - bool _collect_phase{false}; + PX4Rangefinder _px4_rangefinder; int _file_descriptor{-1}; + int _orb_class_instance{-1}; - uint8_t _buffer[sizeof(struct reading_msg)]; + uint8_t _buffer[sizeof(reading_msg)]; uint8_t _buffer_len{0}; - uint8_t _rotation; - hrt_abstime _timeout_usec{0}; + hrt_abstime _measurement_time{0}; - perf_counter_t _collect_timeout_perf{perf_alloc(PC_COUNT, "leddar_one_collect_timeout")}; - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "leddar_one_comms_errors")}; + perf_counter_t _comms_error{perf_alloc(PC_COUNT, "leddar_one_comms_error")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "leddar_one_sample")}; - - orb_advert_t _topic{nullptr}; }; -LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t rotation): +LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation): CDev(device_path), ScheduledWorkItem(px4::wq_configurations::hp_default), - _rotation(rotation) + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation) { _serial_port = strdup(serial_port); + + _px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); + _px4_rangefinder.set_max_distance(LEDDAR_ONE_MAX_DISTANCE); + _px4_rangefinder.set_min_distance(LEDDAR_ONE_MIN_DISTANCE); + _px4_rangefinder.set_fov(LEDDAR_ONE_FIELD_OF_VIEW); + _px4_rangefinder.set_orientation(device_orientation); } LeddarOne::~LeddarOne() { stop(); - free((char *)_serial_port); - - if (_topic) { - orb_unadvertise(_topic); - } - - perf_free(_collect_timeout_perf); - perf_free(_comms_errors); + perf_free(_comms_error); perf_free(_sample_perf); } uint16_t -LeddarOne::crc16_calc(const unsigned char *data_frame, uint8_t crc16_length) +LeddarOne::crc16_calc(const unsigned char *data_frame, const uint8_t crc16_length) { uint16_t crc = 0xFFFF; @@ -228,104 +218,86 @@ LeddarOne::crc16_calc(const unsigned char *data_frame, uint8_t crc16_length) int LeddarOne::collect() { - if (!_collect_phase) { - return measure(); - } - - perf_end(_sample_perf); + perf_begin(_sample_perf); - const hrt_abstime time_now = hrt_absolute_time(); + const int buffer_size = sizeof(_buffer); + const int message_size = sizeof(reading_msg); - struct reading_msg *msg; - - int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, sizeof(_buffer) - _buffer_len); + int bytes_read = ::read(_file_descriptor, _buffer + _buffer_len, buffer_size - _buffer_len); if (bytes_read < 1) { - if (time_now > _timeout_usec) { - _collect_phase = true; - _timeout_usec = 0; - } - - perf_count(_collect_timeout_perf); - return PX4_ERROR; + // Trigger a new measurement. + return measure(); } _buffer_len += bytes_read; - if (_buffer_len < sizeof(struct reading_msg)) { - perf_count(_comms_errors); - return PX4_ERROR; + if (_buffer_len < message_size) { + // Return on next scheduled cycle to collect remaining data. + return PX4_OK; } - msg = (struct reading_msg *)_buffer; + reading_msg *msg {nullptr}; + msg = (reading_msg *)_buffer; - if (msg->slave_addr != MODBUS_SLAVE_ADDRESS || msg->function != MODBUS_READING_FUNCTION) { - perf_count(_comms_errors); - return PX4_ERROR; + if (msg->slave_addr != MODBUS_SLAVE_ADDRESS || + msg->function != MODBUS_READING_FUNCTION) { + PX4_ERR("slave address or function read error"); + perf_count(_comms_error); + perf_end(_sample_perf); + return measure(); } - const uint16_t crc16 = crc16_calc(_buffer, _buffer_len - 2); + const uint16_t crc16 = crc16_calc(_buffer, buffer_size - 2); if (crc16 != msg->crc) { - perf_count(_comms_errors); - return PX4_ERROR; + PX4_ERR("crc error"); + perf_count(_comms_error); + perf_end(_sample_perf); + return measure(); } // NOTE: little-endian support only. - publish(msg->first_dist_high_byte << 8 | msg->first_dist_low_byte); - - _collect_phase = false; - _timeout_usec = time_now + READING_USEC_PERIOD; + uint16_t distance_mm = (msg->first_dist_high_byte << 8 | msg->first_dist_low_byte); + float distance_m = static_cast(distance_mm) / 1000.0f; - perf_end(_sample_perf); - return PX4_OK; -} - -int -LeddarOne::measure() -{ - const hrt_abstime time_now = hrt_absolute_time(); - - if (time_now > _timeout_usec) { - if (request()) { - _buffer_len = 0; - _timeout_usec = time_now + COLLECT_USEC_TIMEOUT; - _collect_phase = true; - return PX4_OK; - } - } + // @TODO - implement a meaningful signal quality value. + int8_t signal_quality = -1; - return PX4_ERROR; -} + _px4_rangefinder.update(_measurement_time, distance_m, signal_quality); -void -LeddarOne::Run() -{ - // Ensure the serial port is open. - open_serial_port(); + perf_end(_sample_perf); - collect(); + // Trigger the next measurement. + return measure();; } int LeddarOne::init() { if (CDev::init()) { - PX4_ERR("Unable to initialize character device\n"); - return -1; + PX4_ERR("Unable to initialize device"); + return PX4_ERROR; } - if (open_serial_port()) { + if (open_serial_port() != PX4_OK) { return PX4_ERROR; } hrt_abstime time_now = hrt_absolute_time(); - hrt_abstime timeout_usec = time_now + PROBE_USEC_TIMEOUT; + + const hrt_abstime timeout_usec = time_now + 500000_us; // 0.5sec while (time_now < timeout_usec) { if (measure() == PX4_OK) { - PX4_INFO("LeddarOne initialized"); - return PX4_OK; + px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL); + + if (collect() == PX4_OK) { + // The file descriptor can only be accessed by the process that opened it, + // so closing here allows the port to be opened from scheduled work queue. + stop(); + return PX4_OK; + } } px4_usleep(1000); @@ -336,6 +308,24 @@ LeddarOne::init() return PX4_ERROR; } +int +LeddarOne::measure() +{ + // Flush the receive buffer. + tcflush(_file_descriptor, TCIFLUSH); + + int num_bytes = ::write(_file_descriptor, request_reading_msg, sizeof(request_reading_msg)); + + if (num_bytes != sizeof(request_reading_msg)) { + PX4_INFO("measurement error: %i, errno: %i", num_bytes, errno); + return PX4_ERROR; + } + + _measurement_time = hrt_absolute_time(); + _buffer_len = 0; + return PX4_OK; +} + int LeddarOne::open_serial_port(const speed_t speed) { @@ -372,12 +362,12 @@ LeddarOne::open_serial_port(const speed_t speed) // Set: 8 data bits, enable receiver, ignore modem status lines. uart_config.c_cflag |= (CS8 | CREAD | CLOCAL); - // Turn off output processing. - uart_config.c_oflag = 0; - // Clear: echo, echo new line, canonical input and extended input. uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN); + // Clear ONLCR flag (which appends a CR for every LF). + uart_config.c_oflag &= ~ONLCR; + // Set the input baud rate in the uart_config struct. int termios_state = cfsetispeed(&uart_config, speed); @@ -405,78 +395,50 @@ LeddarOne::open_serial_port(const speed_t speed) return PX4_ERROR; } + // Flush the hardware buffers. + tcflush(_file_descriptor, TCIOFLUSH); + + PX4_INFO("opened UART port %s", _serial_port); return PX4_OK; } void LeddarOne::print_info() { - - perf_print_counter(_collect_timeout_perf); - perf_print_counter(_comms_errors); + perf_print_counter(_comms_error); perf_print_counter(_sample_perf); - PX4_INFO("measure interval: %u msec", static_cast(WORK_USEC_INTERVAL) / 1000); + PX4_INFO("measure interval: %u msec", static_cast(LEDDAR_ONE_MEASURE_INTERVAL)); } void -LeddarOne::publish(uint16_t distance_mm) -{ - struct distance_sensor_s report; - - report.timestamp = hrt_absolute_time(); - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - report.orientation = _rotation; - report.current_distance = ((float)distance_mm / 1000.0f); - report.min_distance = MIN_DISTANCE; - report.max_distance = MAX_DISTANCE; - report.variance = 0.0f; - report.signal_quality = -1; - report.id = 0; - - if (_topic == nullptr) { - _topic = orb_advertise(ORB_ID(distance_sensor), &report); - - } else { - orb_publish(ORB_ID(distance_sensor), _topic, &report); - } -} - -bool -LeddarOne::request() +LeddarOne::Run() { - /* flush anything in RX buffer */ - tcflush(_file_descriptor, TCIFLUSH); + // Ensure the serial port is open. + open_serial_port(); - int r = ::write(_file_descriptor, request_reading_msg, sizeof(request_reading_msg)); - return r == sizeof(request_reading_msg); + collect(); } void LeddarOne::start() { - /* - * file descriptor can only be accessed by the process that opened it - * so closing here and it will be opened from the High priority kernel - * process - */ - ::close(_file_descriptor); - _file_descriptor = -1; - - ScheduleOnInterval(WORK_USEC_INTERVAL); + // Schedule the driver at regular intervals. + ScheduleOnInterval(LEDDAR_ONE_MEASURE_INTERVAL, LEDDAR_ONE_MEASURE_INTERVAL); + PX4_INFO("driver started"); } void LeddarOne::stop() { - if (_file_descriptor > -1) { - ::close(_file_descriptor); - } + // Ensure the serial port is closed. + ::close(_file_descriptor); + _file_descriptor = -1; + // Clear the work queue schedule. ScheduleClear(); } - /** * Local functions in support of the shell command. */ @@ -485,10 +447,11 @@ namespace leddar_one LeddarOne *g_dev; -int start(const char *port, const uint8_t rotation); +int start(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT, + const uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); int status(); int stop(); -int test(); +int test(const char *port = LEDDAR_ONE_DEFAULT_SERIAL_PORT); int usage(); int start(const char *port, const uint8_t rotation) @@ -501,12 +464,14 @@ int start(const char *port, const uint8_t rotation) g_dev = new LeddarOne(DEVICE_PATH, port, rotation); if (g_dev == nullptr) { + PX4_ERR("object instantiate failed"); delete g_dev; 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; @@ -514,7 +479,6 @@ int start(const char *port, const uint8_t rotation) // Start the driver. g_dev->start(); - PX4_INFO("driver started"); return PX4_OK; } @@ -555,24 +519,37 @@ stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -int test() +int +test(const char *port) { - int fd = open(DEVICE_PATH, O_RDONLY); + // Configure port flags for read/write, non-controlling, non-blocking. + int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK); + + // Open the serial port. + int fd = ::open(port, flags); if (fd < 0) { - PX4_ERR("Unable to open %s", DEVICE_PATH); + PX4_ERR("Unable to open %s", port); return PX4_ERROR; } - distance_sensor_s report; - ssize_t sz = ::read(fd, &report, sizeof(report)); + ssize_t num_bytes = ::write(fd, request_reading_msg, sizeof(request_reading_msg)); - if (sz != sizeof(report)) { - PX4_ERR("No sample available in %s", DEVICE_PATH); + if (num_bytes != sizeof(request_reading_msg)) { + PX4_INFO("serial port write failed: %i, errno: %i", num_bytes, errno); return PX4_ERROR; } - print_message(report); + px4_usleep(LEDDAR_ONE_MEASURE_INTERVAL); + + uint8_t buffer[sizeof(reading_msg)]; + + num_bytes = ::read(fd, buffer, sizeof(reading_msg)); + + if (num_bytes != sizeof(reading_msg)) { + PX4_ERR("Data not available at %s", port); + return PX4_ERROR; + } close(fd); @@ -580,7 +557,8 @@ int test() return PX4_OK; } -int usage() +int +usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( @@ -608,14 +586,12 @@ Stop driver PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("test","Test driver (basic functional tests)"); return PX4_OK; - } } // namespace extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]) { - const char *myoptarg = nullptr; int ch = 0; @@ -647,7 +623,6 @@ extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]) // Start/load the driver. if (!strcmp(argv[myoptind], "start")) { return leddar_one::start(port, rotation); - } // Print the driver status. @@ -658,13 +633,11 @@ extern "C" __EXPORT int leddar_one_main(int argc, char *argv[]) // Stop the driver. if (!strcmp(argv[myoptind], "stop")) { return leddar_one::stop(); - } // Test the driver/device. if (!strcmp(argv[myoptind], "test")) { - return leddar_one::test(); - + return leddar_one::test(port); } // Print driver usage information. diff --git a/src/drivers/distance_sensor/leddar_one/module.yaml b/src/drivers/distance_sensor/leddar_one/module.yaml index cb16327fdfaf..6c70a1af528b 100644 --- a/src/drivers/distance_sensor/leddar_one/module.yaml +++ b/src/drivers/distance_sensor/leddar_one/module.yaml @@ -4,4 +4,3 @@ serial_config: port_config_param: name: SENS_LEDDAR1_CFG group: Sensors - diff --git a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt index 45fd63d64aa6..892b597f0fbb 100644 --- a/src/drivers/distance_sensor/ll40ls/CMakeLists.txt +++ b/src/drivers/distance_sensor/ll40ls/CMakeLists.txt @@ -35,8 +35,6 @@ px4_add_module( MAIN ll40ls COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable - STACK_MAIN - 1500 SRCS ll40ls.cpp LidarLite.cpp diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp index 4c34077e9e86..de71ba941b9f 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.cpp @@ -42,15 +42,10 @@ #include "LidarLite.h" LidarLite::LidarLite(uint8_t rotation) : - _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation), - _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls: read")), - _sample_interval_perf(perf_alloc(PC_ELAPSED, "ll40ls: interval")), - _comms_errors(perf_alloc(PC_COUNT, "ll40ls: comms errors")), - _sensor_resets(perf_alloc(PC_COUNT, "ll40ls: resets")), - _sensor_zero_resets(perf_alloc(PC_COUNT, "ll40ls: zero resets")) + _px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, rotation) { _px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE); - _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V3); + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE); _px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian } diff --git a/src/drivers/distance_sensor/ll40ls/LidarLite.h b/src/drivers/distance_sensor/ll40ls/LidarLite.h index a958f6ea95fd..f77ae5f7dbee 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLite.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLite.h @@ -46,8 +46,9 @@ /* Device limits */ #define LL40LS_MIN_DISTANCE (0.05f) -#define LL40LS_MAX_DISTANCE_V3 (35.00f) -#define LL40LS_MAX_DISTANCE_V1 (25.00f) +#define LL40LS_MAX_DISTANCE (25.00f) +#define LL40LS_MAX_DISTANCE_V2 (35.00f) + // normal conversion wait time #define LL40LS_CONVERSION_INTERVAL 50*1000UL /* 50ms */ @@ -66,35 +67,34 @@ class LidarLite virtual void stop() = 0; /** - * @brief - * Diagnostics - print some basic information about the driver. - */ + * @brief Diagnostics - print some basic information about the driver. + */ void print_info(); /** - * @brief - * print registers to console + * @brief print registers to console */ virtual void print_registers() {}; protected: - uint32_t get_measure_interval() const { return _measure_interval; } + uint32_t get_measure_interval() const { return _measure_interval; } + + virtual int collect() = 0; - virtual int measure() = 0; - virtual int collect() = 0; + virtual int measure() = 0; - virtual int reset_sensor() { return PX4_ERROR; }; + virtual int reset_sensor() { return PX4_ERROR; }; PX4Rangefinder _px4_rangefinder; - perf_counter_t _sample_perf; - perf_counter_t _sample_interval_perf; - perf_counter_t _comms_errors; - perf_counter_t _sensor_resets; - perf_counter_t _sensor_zero_resets; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "ll40ls: comms errors")}; + perf_counter_t _sample_interval_perf{perf_alloc(PC_ELAPSED, "ll40ls: interval")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")}; + perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")}; + perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")}; private: - uint32_t _measure_interval{LL40LS_CONVERSION_INTERVAL}; + uint32_t _measure_interval{LL40LS_CONVERSION_INTERVAL}; }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index 52389ba354c8..45894509d96a 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp @@ -128,43 +128,60 @@ LidarLiteI2C::probe() set_device_address(addresses[i]); - if (addresses[i] == LL40LS_BASEADDR) { + /** + * The probing is divided into different cases. One to detect v2, one for v3 and v1 and one for v3HP. + * The reason for this is that registers are not consistent between different versions. + * The v3HP doesn't have the HW VERSION (or at least no version is specified), + * 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. + */ + + + /** + * check for hw and sw versions for v1, v2 and v3 + */ + if ((read_reg(LL40LS_HW_VERSION, _hw_version) == OK) && + (read_reg(LL40LS_SW_VERSION, _sw_version) == OK)) { - /* - check for unit id. It would be better if - we had a proper WHOAMI register - */ if (read_reg(LL40LS_UNIT_ID_HIGH, id_high) == OK && read_reg(LL40LS_UNIT_ID_LOW, id_low) == OK) { _unit_id = (uint16_t)((id_high << 8) | id_low) & 0xFFFF; - goto ok; } - PX4_DEBUG("probe failed unit_id=0x%02x", (unsigned)_unit_id); + if (_hw_version > 0) { - } else { - /* - check for hw and sw versions. It would be better if - we had a proper WHOAMI register - */ - if (read_reg(LL40LS_HW_VERSION, _hw_version) == OK && _hw_version > 0 && - read_reg(LL40LS_SW_VERSION, _sw_version) == OK && _sw_version > 0) { + if (_unit_id > 0) { + // v2 + PX4_INFO("probe success - hw: %u, sw:%u, id: %u", _hw_version, _sw_version, _unit_id); + _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V2); - _px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE_V1); - goto ok; + } else { + // v1 and v3 + PX4_INFO("probe success - hw: %u, sw:%u", _hw_version, _sw_version); + } + + } else { + + if (_unit_id > 0) { + // v3hp + _is_V3hp = true; + PX4_INFO("probe success - id: %u", _unit_id); + } } - PX4_DEBUG("probe failed hw_version=0x%02x sw_version=0x%02x", (unsigned)_hw_version, (unsigned)_sw_version); + _retries = 3; + return reset_sensor(); } + 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); + } // not found on any address return -EIO; - -ok: - _retries = 3; - return reset_sensor(); } int @@ -180,8 +197,7 @@ LidarLiteI2C::measure() /* * Send the command to begin a measurement. */ - const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE }; - int ret = lidar_transfer(cmd, sizeof(cmd), nullptr, 0); + int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE); if (OK != ret) { perf_count(_comms_errors); @@ -210,12 +226,33 @@ LidarLiteI2C::measure() int LidarLiteI2C::reset_sensor() { - int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); + int ret; + + px4_usleep(15000); + + ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX); if (ret != OK) { return ret; } + px4_usleep(15000); + ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); + + + if (ret != OK) { + uint8_t sig_cnt; + + px4_usleep(15000); + ret = read_reg(LL40LS_SIG_COUNT_VAL_REG, sig_cnt); + + if ((ret != OK) || (sig_cnt != LL40LS_SIG_COUNT_VAL_DEFAULT)) { + PX4_INFO("Error: ll40ls reset failure. Exiting!\n"); + return ret; + + } + } + // wait for sensor reset to complete px4_usleep(50000); ret = write_reg(LL40LS_SIG_COUNT_VAL_REG, LL40LS_SIG_COUNT_VAL_MAX); @@ -354,49 +391,65 @@ int LidarLiteI2C::collect() uint8_t ll40ls_signal_strength = val[0]; + uint8_t signal_min = 0; + uint8_t signal_max = 0; + uint8_t signal_value = 0; - /* 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); + // We detect if V3HP is being used + if (_is_V3hp) { + signal_min = LL40LS_SIGNAL_STRENGTH_MIN_V3HP; + signal_max = LL40LS_SIGNAL_STRENGTH_MAX_V3HP; + signal_value = ll40ls_signal_strength; - // 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 strenght read failed"); + } 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); + + // 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(); + } + } - DEVICE_DEBUG("error reading peak strength from sensor: %d", ret); - perf_count(_comms_errors); + perf_end(_sample_perf); + // if we are getting lots of I2C transfer errors try + // resetting the sensor + return ret; + } - if (perf_event_count(_comms_errors) % 10 == 0) { - perf_count(_sensor_resets); - reset_sensor(); - } + uint8_t ll40ls_peak_strength = val[0]; + signal_min = LL40LS_PEAK_STRENGTH_LOW; + signal_max = LL40LS_PEAK_STRENGTH_HIGH; + + // 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) { + signal_value = 0; + + } else { + signal_value = ll40ls_peak_strength; } - 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]; - /* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/ // Step 1: Normalize signal strength to 0...100 percent using the absolute signal peak strength. - uint8_t signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW, - 0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW); - - // Step 2: Also 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) { - signal_quality = 0; - } + uint8_t signal_quality = 100 * math::max(signal_value - signal_min, 0) / (signal_max - signal_min); - // Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS. + // Step 2: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS. if (distance_m < LL40LS_MIN_DISTANCE) { signal_quality = 0; } diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h index 38fcd7afe9dc..0aa7d85699a4 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.h @@ -42,13 +42,14 @@ #include "LidarLite.h" -#include +#include #include /* Configuration Constants */ static constexpr uint8_t LL40LS_BASEADDR = 0x62; /* 7-bit address */ static constexpr uint8_t LL40LS_BASEADDR_OLD = 0x42; /* previous 7-bit address */ +static constexpr uint8_t LL40LS_SIG_COUNT_VAL_DEFAULT = 0x80; /* Default maximum acquisition count */ /* LL40LS Registers addresses */ static constexpr uint8_t LL40LS_MEASURE_REG = 0x00; /* Measure range register */ @@ -66,6 +67,8 @@ static constexpr uint8_t LL40LS_UNIT_ID_LOW = 0x17; static constexpr uint8_t LL40LS_SIG_COUNT_VAL_REG = 0x02; /* Maximum acquisition count register */ static constexpr uint8_t LL40LS_SIG_COUNT_VAL_MAX = 0xFF; /* Maximum acquisition count max value */ +static constexpr int LL40LS_SIGNAL_STRENGTH_MIN_V3HP = 70; // Min signal strength for V3HP +static constexpr int LL40LS_SIGNAL_STRENGTH_MAX_V3HP = 255; // Max signal strength for V3HP static constexpr int LL40LS_SIGNAL_STRENGTH_LOW = 24; // Minimum (relative) signal strength value for accepting a measurement @@ -147,5 +150,6 @@ class LidarLiteI2C : public LidarLite, public device::I2C, public px4::Scheduled uint8_t _hw_version{0}; uint8_t _sw_version{0}; uint16_t _unit_id{0}; + bool _is_V3hp{false}; }; diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h index de37f581d3bc..d50c5b54a45e 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h @@ -45,7 +45,7 @@ #include "LidarLite.h" -#include +#include #include class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem diff --git a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp index 847b9c805246..ecc90ed2f327 100644 --- a/src/drivers/distance_sensor/ll40ls/ll40ls.cpp +++ b/src/drivers/distance_sensor/ll40ls/ll40ls.cpp @@ -89,7 +89,7 @@ namespace ll40ls { LidarLite *instance = nullptr; - +int start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation); void start(enum LL40LS_BUS busid, uint8_t rotation); void stop(); void info(); @@ -126,35 +126,53 @@ void start(enum LL40LS_BUS busid, uint8_t rotation) continue; } - instance = new LidarLiteI2C(bus_options[i].busnum, rotation); - - if (!instance) { - PX4_ERR("Failed to instantiate LidarLiteI2C"); + if (start_bus(bus_options[i], rotation) == PX4_OK) { return; } - if (instance->init() == PX4_OK) { - break; - } - - PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum); - stop(); } } - if (!instance) { + if (instance == nullptr) { PX4_WARN("No LidarLite found"); return; } } +/** + * Start the driver on a specific bus. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. + */ +int +start_bus(const struct ll40ls_bus_option &i2c_bus, uint8_t rotation) +{ + instance = new LidarLiteI2C(i2c_bus.busnum, rotation); + + if (instance->init() != OK) { + stop(); + PX4_INFO("LidarLiteI2C - no device on bus %u", (unsigned)i2c_bus.busid); + return PX4_ERROR; + } + + return PX4_OK; +} + /** * @brief Stops the driver */ void stop() { - delete instance; - instance = nullptr; + if (instance != nullptr) { + delete instance; + instance = nullptr; + + } else { + PX4_ERR("driver not running"); + } + + } /** diff --git a/src/drivers/distance_sensor/mappydot/CMakeLists.txt b/src/drivers/distance_sensor/mappydot/CMakeLists.txt index 51a90f77c7a2..089bf7422245 100644 --- a/src/drivers/distance_sensor/mappydot/CMakeLists.txt +++ b/src/drivers/distance_sensor/mappydot/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__mappydot MAIN mappydot - STACK_MAIN 1200 SRCS MappyDot.cpp ) diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index c5e858b4ba81..3500908ab0b1 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include /* MappyDot Registers */ diff --git a/src/drivers/distance_sensor/mb12xx/CMakeLists.txt b/src/drivers/distance_sensor/mb12xx/CMakeLists.txt index 67fb66acd1ab..9d1dffd05975 100644 --- a/src/drivers/distance_sensor/mb12xx/CMakeLists.txt +++ b/src/drivers/distance_sensor/mb12xx/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__mb12xx MAIN mb12xx - STACK_MAIN 1400 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index cc180c703e5e..3541062eae0c 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index f454ea7cd3a0..817461560bcb 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 9ab43c864522..f2d38846a2d7 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/distance_sensor/srf02/CMakeLists.txt b/src/drivers/distance_sensor/srf02/CMakeLists.txt index 5f80b27be718..dd412dcdfc39 100644 --- a/src/drivers/distance_sensor/srf02/CMakeLists.txt +++ b/src/drivers/distance_sensor/srf02/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__srf02 MAIN srf02 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index b2e653962249..ac0c67982588 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/distance_sensor/teraranger/CMakeLists.txt b/src/drivers/distance_sensor/teraranger/CMakeLists.txt index e3edbf7f6cfd..f74bb1f65dc4 100644 --- a/src/drivers/distance_sensor/teraranger/CMakeLists.txt +++ b/src/drivers/distance_sensor/teraranger/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__teraranger MAIN teraranger - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 6808ab3f7fce..14851308953f 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index f2272efd61a1..c1f48d86a249 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -66,7 +66,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/distance_sensor/ulanding/ulanding.cpp b/src/drivers/distance_sensor/ulanding/ulanding.cpp index 58abd3e2e326..5ba754961383 100644 --- a/src/drivers/distance_sensor/ulanding/ulanding.cpp +++ b/src/drivers/distance_sensor/ulanding/ulanding.cpp @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp index 215b9c9c29b0..f64131583374 100644 --- a/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp +++ b/src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include /* Configuration Constants */ diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt index 50967ea90f69..dc9db8bd9660 100644 --- a/src/platforms/posix/drivers/df_ak8963_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_ak8963_wrapper diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_bebop_bus_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_bebop_bus_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt index 997261971561..bd2fada0eec6 100644 --- a/src/platforms/posix/drivers/df_bebop_bus_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_bebop_bus_wrapper diff --git a/src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/CMakeLists.txt index 4ce56a381349..5273142396eb 100644 --- a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_bebop_rangefinder_wrapper diff --git a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_bmp280_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_bmp280_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_bmp280_wrapper/CMakeLists.txt index de214f461625..b16e02e3cf40 100644 --- a/src/platforms/posix/drivers/df_bmp280_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_bmp280_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_bmp280_wrapper diff --git a/src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_bmp280_wrapper/df_bmp280_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_bmp280_wrapper/df_bmp280_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_bmp280_wrapper/df_bmp280_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_hmc5883_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt index 03c76ed090aa..b19f4eefc138 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_hmc5883_wrapper diff --git a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_isl29501_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt index 373d1db61bf4..b48e85238c13 100644 --- a/src/platforms/posix/drivers/df_isl29501_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_isl29501_wrapper diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt index 732421a2558e..329b4cd21ae9 100644 --- a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_lsm9ds1_wrapper diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_ltc2946_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_ltc2946_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt index 6095aa793ec9..292c8a5aa772 100644 --- a/src/platforms/posix/drivers/df_ltc2946_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_ltc2946_wrapper diff --git a/src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt index 4147bf78db9d..3ec392d85463 100644 --- a/src/platforms/posix/drivers/df_mpu6050_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_mpu6050_wrapper diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_mpu9250_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt index 9f60ac1312e7..f5f87eb75422 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_mpu9250_wrapper diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt index 9756a6d16435..f0afda81fa0c 100644 --- a/src/platforms/posix/drivers/df_ms5607_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_ms5607_wrapper diff --git a/src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_ms5607_wrapper/df_ms5607_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_ms5607_wrapper/df_ms5607_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt index d21aa5733cef..e18fd8567973 100644 --- a/src/platforms/posix/drivers/df_ms5611_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_ms5611_wrapper diff --git a/src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_ms5611_wrapper/df_ms5611_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_ms5611_wrapper/df_ms5611_wrapper.cpp diff --git a/src/platforms/posix/drivers/df_trone_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt similarity index 96% rename from src/platforms/posix/drivers/df_trone_wrapper/CMakeLists.txt rename to src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt index 30b9974c0248..2b6d79e8214b 100644 --- a/src/platforms/posix/drivers/df_trone_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -include_directories(../../../../lib/DriverFramework/drivers) +include_directories(../../../lib/DriverFramework/drivers) px4_add_module( MODULE platforms__posix__drivers__df_trone_wrapper diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp similarity index 100% rename from src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp rename to src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp diff --git a/src/drivers/drv_adc.h b/src/drivers/drv_adc.h index 4fdfa2d6a9e9..3e7e984eafc7 100644 --- a/src/drivers/drv_adc.h +++ b/src/drivers/drv_adc.h @@ -36,8 +36,6 @@ * * ADC driver interface. * - * This defines additional operations over and above the standard NuttX - * ADC API. */ #pragma once @@ -60,6 +58,34 @@ typedef struct __attribute__((packed)) px4_adc_msg_t { #define ADC0_DEVICE_PATH "/dev/adc0" -/* - * ioctl definitions + +__BEGIN_DECLS + +/** + * Initialize ADC hardware + * @param base_address architecture-specific address to specify the ADC + * @return 0 on success, <0 error otherwise + */ +int px4_arch_adc_init(uint32_t base_address); + +/** + * Uninitialize ADC hardware + * @param base_address architecture-specific address to specify the ADC + */ +void px4_arch_adc_uninit(uint32_t base_address); + +/** + * Read a sample from the ADC + * @param base_address architecture-specific address to specify the ADC + * @param channel specify the channel + * @return sample, 0xffff on error */ +uint16_t px4_arch_adc_sample(uint32_t base_address, unsigned channel); + +/** + * Get the temperature sensor channel bitmask + */ +uint32_t px4_arch_adc_temp_sensor_mask(void); + +__END_DECLS + diff --git a/src/drivers/drv_led.h b/src/drivers/drv_led.h index e02bb4fa2448..53dc01c2d26e 100644 --- a/src/drivers/drv_led.h +++ b/src/drivers/drv_led.h @@ -49,14 +49,12 @@ #define BOARD_MAX_LEDS 4 #endif +static_assert(led_control_s::ORB_QUEUE_LENGTH >= BOARD_MAX_LEDS, "led_control_s::ORB_QUEUE_LENGTH too small"); + #if BOARD_MAX_LEDS > 8 // because led_mask is uint8_t #error "BOARD_MAX_LEDS too large. You need to change the led_mask type in the led_control uorb topic (and where it's used)" #endif - -// set the queue size to the number of LED's, so that each led can be controlled individually -static const int LED_UORB_QUEUE_LENGTH = BOARD_MAX_LEDS; - // Legacy paths - 2 are need to allow both pwm and i2c drviers to co-exist #define RGBLED0_DEVICE_PATH "/dev/rgbled0" // Primary RGB LED on i2c #define RGBLED1_DEVICE_PATH "/dev/rgbled1" // Primary RGB LED(NCP5623C) on i2c diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h deleted file mode 100644 index 8be6eb8d1ac2..000000000000 --- a/src/drivers/drv_oreoled.h +++ /dev/null @@ -1,219 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-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_oreoled.h - * - * Oreo led device API - */ - -#pragma once - -#include -#include - -/* oreoled device path */ -#define OREOLED0_DEVICE_PATH "/dev/oreoled0" - -/* - * ioctl() definitions - */ - -#define _OREOLEDIOCBASE (0x2d00) -#define _OREOLEDIOC(_n) (_IOC(_OREOLEDIOCBASE, _n)) - -/** set constant RGB values */ -#define OREOLED_SET_RGB _OREOLEDIOC(1) - -/** run macro */ -#define OREOLED_RUN_MACRO _OREOLEDIOC(2) - -/** send bytes */ -#define OREOLED_SEND_BYTES _OREOLEDIOC(3) - -/** send reset */ -#define OREOLED_SEND_RESET _OREOLEDIOC(4) - -/** boot ping */ -#define OREOLED_BL_PING _OREOLEDIOC(5) - -/** boot version */ -#define OREOLED_BL_VER _OREOLEDIOC(6) - -/** boot write flash */ -#define OREOLED_BL_FLASH _OREOLEDIOC(7) - -/** boot application version */ -#define OREOLED_BL_APP_VER _OREOLEDIOC(8) - -/** boot application crc */ -#define OREOLED_BL_APP_CRC _OREOLEDIOC(9) - -/** boot startup colour */ -#define OREOLED_BL_SET_COLOUR _OREOLEDIOC(10) - -/** boot application */ -#define OREOLED_BL_BOOT_APP _OREOLEDIOC(11) - -/** force an i2c gencall */ -#define OREOLED_FORCE_SYNC _OREOLEDIOC(12) - -/* Oreo LED driver supports up to 4 leds */ -#define OREOLED_NUM_LEDS 4 - -/* instance of 0xff means apply to all instances */ -#define OREOLED_ALL_INSTANCES 0xff - -/* maximum command length that can be sent to LEDs */ -#define OREOLED_CMD_LENGTH_MAX 70 - -/* maximum command length that can be read from LEDs */ -#define OREOLED_CMD_READ_LENGTH_MAX 10 - -/* maximum number of commands retries */ -#define OEROLED_COMMAND_RETRIES 10 - -/* magic number used to verify the software reset is valid */ -#define OEROLED_RESET_NONCE 0x2A - -/* microseconds to hold-off between write and reads */ -#define OREOLED_WRITE_READ_HOLDOFF_US 500 - -/* microseconds to hold-off between retries */ -#define OREOLED_RETRY_HOLDOFF_US 200 - -#define OEROLED_BOOT_COMMAND_RETRIES 25 -#define OREOLED_BOOT_FLASH_WAITMS 10 - -#define OREOLED_BOOT_SUPPORTED_VER 0x01 - -#define OREOLED_BOOT_CMD_PING 0x40 -#define OREOLED_BOOT_CMD_BL_VER 0x41 -#define OREOLED_BOOT_CMD_APP_VER 0x42 -#define OREOLED_BOOT_CMD_APP_CRC 0x43 -#define OREOLED_BOOT_CMD_SET_COLOUR 0x44 - -#define OREOLED_BOOT_CMD_WRITE_FLASH_A 0x50 -#define OREOLED_BOOT_CMD_WRITE_FLASH_B 0x51 -#define OREOLED_BOOT_CMD_FINALISE_FLASH 0x55 - -#define OREOLED_BOOT_CMD_BOOT_APP 0x60 - -#define OREOLED_BOOT_CMD_PING_NONCE 0x2A -#define OREOLED_BOOT_CMD_BOOT_NONCE 0xA2 - -#define OREOLED_FW_FILE_HEADER_LENGTH 2 -#define OREOLED_FW_FILE_SIZE_LIMIT 6144 -#define OREOLED_FW_FILE "/etc/firmware/oreoled.bin" - -/* enum passed to OREOLED_SET_MODE ioctl() - * defined by hardware */ -enum oreoled_pattern { - OREOLED_PATTERN_OFF = 0, - OREOLED_PATTERN_SINE = 1, - OREOLED_PATTERN_SOLID = 2, - OREOLED_PATTERN_SIREN = 3, - OREOLED_PATTERN_STROBE = 4, - OREOLED_PATTERN_FADEIN = 5, - OREOLED_PATTERN_FADEOUT = 6, - OREOLED_PATTERN_PARAMUPDATE = 7, - OREOLED_PATTERN_ENUM_COUNT -}; - -/* enum passed to OREOLED_SET_MODE ioctl() - * defined by hardware */ -enum oreoled_param { - OREOLED_PARAM_BIAS_RED = 0, - OREOLED_PARAM_BIAS_GREEN = 1, - OREOLED_PARAM_BIAS_BLUE = 2, - OREOLED_PARAM_AMPLITUDE_RED = 3, - OREOLED_PARAM_AMPLITUDE_GREEN = 4, - OREOLED_PARAM_AMPLITUDE_BLUE = 5, - OREOLED_PARAM_PERIOD = 6, - OREOLED_PARAM_REPEAT = 7, - OREOLED_PARAM_PHASEOFFSET = 8, - OREOLED_PARAM_MACRO = 9, - OREOLED_PARAM_RESET = 10, - OREOLED_PARAM_APP_CHECKSUM = 11, - OREOLED_PARAM_ENUM_COUNT -}; - -/* enum of available macros - * defined by hardware */ -enum oreoled_macro { - OREOLED_PARAM_MACRO_RESET = 0, - OREOLED_PARAM_MACRO_COLOUR_CYCLE = 1, - OREOLED_PARAM_MACRO_BREATH = 2, - OREOLED_PARAM_MACRO_STROBE = 3, - OREOLED_PARAM_MACRO_FADEIN = 4, - OREOLED_PARAM_MACRO_FADEOUT = 5, - OREOLED_PARAM_MACRO_RED = 6, - OREOLED_PARAM_MACRO_GREEN = 7, - OREOLED_PARAM_MACRO_BLUE = 8, - OREOLED_PARAM_MACRO_YELLOW = 9, - OREOLED_PARAM_MACRO_WHITE = 10, - OREOLED_PARAM_MACRO_AUTOMOBILE = 11, - OREOLED_PARAM_MACRO_AVIATION = 12, - OREOLED_PARAM_MACRO_ENUM_COUNT -}; - -/* - structure passed to OREOLED_SET_RGB ioctl() - Note that the driver scales the brightness to 0 to 255, regardless - of the hardware scaling - */ -typedef struct { - uint8_t instance; - oreoled_pattern pattern; - uint8_t red; - uint8_t green; - uint8_t blue; -} oreoled_rgbset_t; - -/* - structure passed to OREOLED_RUN_MACRO ioctl() - */ -typedef struct { - uint8_t instance; - oreoled_macro macro; -} oreoled_macrorun_t; - -/* - structure passed to send_bytes method (only used for testing) - */ -typedef struct { - uint8_t led_num; - uint8_t num_bytes; - uint8_t buff[OREOLED_CMD_LENGTH_MAX]; -} oreoled_cmd_t; - diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5428e0871a68..5f414f428ab4 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -61,6 +61,7 @@ #define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_MAG_DEVTYPE_QMC5883 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 +#define DRV_DEVTYPE_ICM20948 0x0A #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 @@ -100,7 +101,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x50 #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 -#define DRV_MAG_DEVTYPE_FXOS8701C 0x53 + #define DRV_GYR_DEVTYPE_FXAS2100C 0x54 #define DRV_ACC_DEVTYPE_ADIS16448 0x55 #define DRV_MAG_DEVTYPE_ADIS16448 0x56 @@ -113,6 +114,7 @@ #define DRV_ACC_DEVTYPE_ADIS16497 0x63 #define DRV_GYR_DEVTYPE_ADIS16497 0x64 #define DRV_BARO_DEVTYPE_BAROSIM 0x65 +#define DRV_DEVTYPE_BMI088 0x66 /* * ioctl() definitions diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 7d389cd7fe30..f22d4798422c 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -91,3 +91,23 @@ enum { TONE_HOME_SET, TONE_NUMBER_OF_TUNES }; + +namespace ToneAlarmInterface +{ + +/** + * @brief Activates/configures the hardware registers. + */ +void init(); + +/** + * @brief Starts playing the note. + */ +void start_note(unsigned frequency); + +/** + * @brief Stops playing the current note and makes the player 'safe'. + */ +void stop_note(); + +} // ToneAlarmInterface diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 88846a63e886..f07fac969c62 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_git_submodule(TARGET git_gps_devices PATH "devices") px4_add_module( MODULE drivers__gps MAIN gps - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3979a267cebc..e291bbd45d50 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -172,9 +173,10 @@ class GPS : public ModuleBase const Instance _instance; uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; - orb_advert_t _dump_communication_pub{nullptr}; ///< if non-null, dump communication + uORB::PublicationQueued _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 static volatile 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. @@ -414,7 +416,15 @@ void GPS::handleInjectDataTopic() { bool updated = false; + // Limit maximum number of GPS injections to 6 since usually + // GPS injections should consist of 1-4 packets (GPS, Glonass, Baidu, Galileo). + // Looking at 6 packets thus guarantees, that at least a full injection + // data set is evaluated. + const size_t max_num_injections = 6; + size_t num_injections = 0; + do { + num_injections++; updated = _orb_inject_data_sub.updated(); if (updated) { @@ -429,7 +439,7 @@ void GPS::handleInjectDataTopic() ++_last_rate_rtcm_injection_count; } - } while (updated); + } while (updated && num_injections < max_num_injections); } bool GPS::injectData(uint8_t *data, size_t len) @@ -549,16 +559,16 @@ void GPS::initializeCommunicationDump() memset(_dump_to_device, 0, sizeof(gps_dump_s)); memset(_dump_from_device, 0, sizeof(gps_dump_s)); - int instance; //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 = orb_advertise_multi_queue(ORB_ID(gps_dump), _dump_from_device, &instance, - ORB_PRIO_DEFAULT, 8); + _dump_communication_pub.publish(*_dump_from_device); + + _should_dump_communication = true; } void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) { - if (!_dump_communication_pub) { + if (!_should_dump_communication) { return; } @@ -582,7 +592,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) } dump_data->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(gps_dump), _dump_communication_pub, dump_data); + _dump_communication_pub.publish(*dump_data); dump_data->len = 0; } } @@ -820,10 +830,6 @@ GPS::run() PX4_INFO("exiting"); - if (_dump_communication_pub) { - orb_unadvertise(_dump_communication_pub); - } - if (_serial_fd >= 0) { ::close(_serial_fd); _serial_fd = -1; @@ -1062,7 +1068,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) } int task_id = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1600, + SCHED_PRIORITY_SLOW_DRIVER, 1700, entry_point, (char *const *)argv); if (task_id < 0) { diff --git a/src/drivers/heater/CMakeLists.txt b/src/drivers/heater/CMakeLists.txt index a81d51ee0bc5..6f1fe2e249a1 100644 --- a/src/drivers/heater/CMakeLists.txt +++ b/src/drivers/heater/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__heater MAIN heater - STACK_MAIN 1024 COMPILE_FLAGS SRCS heater.cpp diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index bcc1916f0175..e492d570f6c4 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -45,7 +45,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/imu/adis16448/ADIS16448.h b/src/drivers/imu/adis16448/ADIS16448.h index e1c01842baee..e5fd10d8e2d1 100644 --- a/src/drivers/imu/adis16448/ADIS16448.h +++ b/src/drivers/imu/adis16448/ADIS16448.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include using namespace time_literals; diff --git a/src/drivers/imu/adis16448/CMakeLists.txt b/src/drivers/imu/adis16448/CMakeLists.txt index 1b7d18551e46..4f4463fb03d8 100644 --- a/src/drivers/imu/adis16448/CMakeLists.txt +++ b/src/drivers/imu/adis16448/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__imu__adis16448 MAIN adis16448 - STACK_MAIN 1400 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index a5ed6c562bdd..cc7cec9dd67b 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include class ADIS16477 : public device::SPI, public px4::ScheduledWorkItem { diff --git a/src/drivers/imu/adis16477/CMakeLists.txt b/src/drivers/imu/adis16477/CMakeLists.txt index 132dd77cabf6..fb12c05ec54b 100644 --- a/src/drivers/imu/adis16477/CMakeLists.txt +++ b/src/drivers/imu/adis16477/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__imu__adis16477 MAIN adis16477 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/adis16497/ADIS16497.hpp b/src/drivers/imu/adis16497/ADIS16497.hpp index 8894d9d778b4..7cb0e7fdf595 100644 --- a/src/drivers/imu/adis16497/ADIS16497.hpp +++ b/src/drivers/imu/adis16497/ADIS16497.hpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include // TODO : This is a copy of the NuttX CRC32 table static constexpr uint32_t crc32_tab[] = { diff --git a/src/drivers/imu/adis16497/CMakeLists.txt b/src/drivers/imu/adis16497/CMakeLists.txt index 10c0e99d1895..027d89280a1e 100644 --- a/src/drivers/imu/adis16497/CMakeLists.txt +++ b/src/drivers/imu/adis16497/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__imu__adis16497 MAIN adis16497 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index 47dfba02941d..ea63dd20d4ed 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -66,7 +66,7 @@ #include #include #include -#include +#include #define ACCEL_DEVICE_PATH "/dev/bma180" diff --git a/src/drivers/imu/bmi055/BMI055.hpp b/src/drivers/imu/bmi055/BMI055.hpp index eb11619b64c9..75e28f683184 100644 --- a/src/drivers/imu/bmi055/BMI055.hpp +++ b/src/drivers/imu/bmi055/BMI055.hpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 diff --git a/src/drivers/imu/bmi055/CMakeLists.txt b/src/drivers/imu/bmi055/CMakeLists.txt index 049d325fd570..92b6d34da64c 100644 --- a/src/drivers/imu/bmi055/CMakeLists.txt +++ b/src/drivers/imu/bmi055/CMakeLists.txt @@ -33,10 +33,9 @@ px4_add_module( MODULE drivers__bmi055 MAIN bmi055 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable - SRCS + SRCS BMI055_accel.cpp BMI055_gyro.cpp bmi055_main.cpp diff --git a/src/drivers/imu/bmi088/BMI088.hpp b/src/drivers/imu/bmi088/BMI088.hpp new file mode 100644 index 000000000000..8e3e6a904e68 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +//Soft-reset command Value +#define BMI088_SOFT_RESET 0xB6 + +#define BMI088_BUS_SPEED 10*1000*1000 + +#define BMI088_TIMER_REDUCTION 200 + +class BMI088 : public device::SPI +{ + +protected: + + uint8_t _whoami; /** whoami result */ + + uint8_t _register_wait; + uint64_t _reset_wait; + + enum Rotation _rotation; + + uint8_t _checked_next; + + /** + * Read a register from the BMI088 + * + * @param The register to read. + * @return The value that was read. + */ + virtual uint8_t read_reg(unsigned reg); // This needs to be declared as virtual, because the + virtual uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the BMI088 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /* do not allow to copy this class due to pointer data members */ + BMI088(const BMI088 &); + BMI088 operator=(const BMI088 &); + +public: + + BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency, + enum Rotation rotation); + + virtual ~BMI088() = default; + + +}; diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp new file mode 100644 index 000000000000..14d4320870a5 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -0,0 +1,614 @@ +/**************************************************************************** + * + * 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 "BMI088_accel.hpp" + +/* + * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. The variable is changed in bmi055_accel.cpp. + * This is a HACK! The driver should be rewritten with the gyro as subdriver. + */ +extern float _accel_last_temperature_copy; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS] = {BMI088_ACC_CHIP_ID, + BMI088_ACC_CONF, + BMI088_ACC_RANGE, + BMI088_ACC_INT1_IO_CONF, + BMI088_ACC_INT1_INT2_MAP_DATA, + BMI088_ACC_PWR_CONF, + BMI088_ACC_PWR_CTRL, + }; + +BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : + BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")), + _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_accel_measure_interval")), + _bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")), + _duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")), + _got_duplicate(false) +{ + _px4_accel.set_device_type(DRV_DEVTYPE_BMI088); +} + +BMI088_accel::~BMI088_accel() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_measure_interval); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_duplicates); +} + +int +BMI088_accel::init() +{ + /* do SPI init (and probe) first */ + int ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + return reset(); +} + +uint8_t +BMI088_accel::read_reg(unsigned reg) +{ + // For the BMI088, you need to read out a dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0}; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[2]; // Skip dummy byte in cmd[1] and read out actual data +} + +uint16_t +BMI088_accel::read_reg16(unsigned reg) +{ + // For the BMI088, you need to read out the dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + uint8_t cmd[4] = { (uint8_t)(reg | DIR_READ), 0, 0, 0 }; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[2] << 8) | cmd[3]; // Skip dummy byte in cmd[1] +} + +int BMI088_accel::reset() +{ + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); // Soft-reset + /* After a POR or soft-reset, the sensor needs up to 1ms boot time + * (see section "Power Modes: Accelerometer" in the BMI088 datasheet). + * Based off of testing it seems this value needs to be increased from 1ms. + * Setting it to 5ms. + */ + + up_udelay(5000); + + // Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset + // The dummy read basically pulls the chip select line low and then high + // See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details. + read_reg(BMI088_ACC_CHIP_ID); + + // Enable Accelerometer + // The accelerometer needs to be enabled first, before writing to its registers + write_checked_reg(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_EN); + /* After changing power modes, the sensor requires up to 5ms to settle. + * Any communication with the sensor during this time should be avoided + * (see section "Power Modes: Acceleromter" in the BMI datasheet) */ + + up_udelay(5000); + + // Set the PWR CONF to be active + write_checked_reg(BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_CONF_ACTIVE); // Sets the accelerometer to active mode + + // Write accel bandwidth and output data rate + // ToDo set the bandwidth + accel_set_sample_rate(BMI088_ACCEL_DEFAULT_RATE); //set accel ODR + set_accel_range(BMI088_ACCEL_DEFAULT_RANGE_G); //set accel range + + // Configure the accel INT1 + write_checked_reg(BMI088_ACC_INT1_IO_CONF, + BMI088_ACC_INT1_IO_CONF_INT1_OUT | BMI088_ACC_INT1_IO_CONF_PP | + BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH); // Configure INT1 pin as output, push-pull, active high + write_checked_reg(BMI088_ACC_INT1_INT2_MAP_DATA, + BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY); // Map DRDY interrupt on pin INT1 + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + return OK; +} + +int +BMI088_accel::probe() +{ + // Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset + // The dummy read basically pulls the chip select line low and then high + // See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details. + read_reg(BMI088_ACC_CHIP_ID); + + /* look for device ID */ + _whoami = read_reg(BMI088_ACC_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI088_ACC_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + return -EIO; +} + +int +BMI088_accel::accel_set_sample_rate(float frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = 0x0F; + + if (frequency < 25) { + setbits |= BMI088_ACC_CONF_ODR_12_5; + //_accel_sample_rate = 12.5f; + + } else if (frequency < 50) { + setbits |= BMI088_ACC_CONF_ODR_25; + //_accel_sample_rate = 25.f; + + } else if (frequency < 100) { + setbits |= BMI088_ACC_CONF_ODR_50; + //_accel_sample_rate = 50.f; + + } else if (frequency < 200) { + setbits |= BMI088_ACC_CONF_ODR_100; + //_accel_sample_rate = 100.f; + + } else if (frequency < 400) { + setbits |= BMI088_ACC_CONF_ODR_200; + //_accel_sample_rate = 200.f; + + } else if (frequency < 800) { + setbits |= BMI088_ACC_CONF_ODR_400; + //_accel_sample_rate = 400.f; + + } else if (frequency < 1600) { + setbits |= BMI088_ACC_CONF_ODR_800; + //_accel_sample_rate = 800.f; + + } else if (frequency >= 1600) { + setbits |= BMI088_ACC_CONF_ODR_1600; + //_accel_sample_rate = 1600.f; + + } else { + printf("Set sample rate error \n"); + return -EINVAL; + } + + /* Write accel ODR */ + modify_reg(BMI088_ACC_CONF, clearbits, setbits); + + return OK; +} + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI088_accel::test_error() +{ + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + +void +BMI088_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI088_accel::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + +int +BMI088_accel::set_accel_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_ACCEL_RANGE_24_G; + float lsb_per_g; + + if (max_g == 0) { + max_g = 24; + } + + if (max_g <= 3) { + //max_accel_g = 3; + setbits |= BMI088_ACCEL_RANGE_3_G; + lsb_per_g = 10922.67; + + } else if (max_g <= 6) { + //max_accel_g = 6; + setbits |= BMI088_ACCEL_RANGE_6_G; + lsb_per_g = 5461.33; + + } else if (max_g <= 12) { + //max_accel_g = 12; + setbits |= BMI088_ACCEL_RANGE_12_G; + lsb_per_g = 2730.67; + + } else if (max_g <= 24) { + //max_accel_g = 24; + setbits |= BMI088_ACCEL_RANGE_24_G; + lsb_per_g = 1365.33; + + } else { + return -EINVAL; + } + + _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); + + modify_reg(BMI088_ACC_RANGE, clearbits, setbits); + + return OK; +} + +void +BMI088_accel::start() +{ + /* make sure we are stopped first */ + stop(); + + // Reset the accelerometer + reset(); + + /* start polling at the specified rate */ + ScheduleOnInterval(BMI088_ACCEL_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000); + +} + +void +BMI088_accel::stop() +{ + ScheduleClear(); +} + +void +BMI088_accel::Run() +{ + /* make another measurement */ + measure(); +} + +void +BMI088_accel::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI088_ACCEL_NUM_CHECKED_REGISTERS; +} + +void +BMI088_accel::measure() +{ + perf_count(_measure_interval); + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + // Checking the status of new data + uint8_t status; + status = read_reg(BMI088_ACC_STATUS); + + if (!(status & BMI088_ACC_STATUS_DRDY)) { + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + + _got_duplicate = false; + + /* + * Fetch the full set of measurements from the BMI088 in one pass. + */ + uint8_t index = 0; + uint8_t accel_data[8]; // Need an extra byte for the command, and an an extra dummy byte for the read (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + accel_data[index] = BMI088_ACC_X_L | DIR_READ; + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) { + return; + } + + check_registers(); + + /* Extracting accel data from the read data */ + index = 2; // Skip the dummy byte at index=1 + uint16_t lsb, msb, msblsb; + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_x = (int16_t)msblsb; /* Data in X axis */ + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_y = (int16_t)msblsb; /* Data in Y axis */ + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_z = (int16_t)msblsb; /* Data in Z axis */ + + // Extract the temperature data + // Note: the temp sensor data is only updated every 1.28s (see "Register 0x22-0x23 Temperature Sensor Data" section in BMI088 Datasheet) + index = 0; + accel_data[index] = BMI088_ACC_TEMP_H | DIR_READ; + + // Need to perform a dummy read, hence the num bytes to read is 3 (plus 1 send byte) + if (OK != transfer(accel_data, accel_data, 4)) { + return; + } + + index = 2; + msb = (uint16_t)accel_data[index++]; + lsb = (uint16_t)accel_data[index++]; + uint16_t temp = msb * 8 + lsb / 32; + + if (temp > 1023) { + report.temp = temp - 2048; + + } else { + report.temp = temp; + } + + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi088 accel does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again, but don't return any data yet + _register_wait--; + return; + } + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + + // Convert the bit-wise representation of temperature to degrees C + _accel_last_temperature_copy = (report.temp * 0.125f) + 23.0f; + _px4_accel.set_temperature(_accel_last_temperature_copy); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + */ + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +BMI088_accel::print_info() +{ + PX4_INFO("Accel"); + + perf_print_counter(_sample_perf); + perf_print_counter(_measure_interval); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_duplicates); + + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + _px4_accel.print_status(); +} + +void +BMI088_accel::print_registers() +{ + uint8_t index = 0; + printf("BMI088 accel registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int1 Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int1-Int2_Map-Data: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Pwr Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Pwr Ctrl: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + + + printf("\n"); +} diff --git a/src/drivers/imu/bmi088/BMI088_accel.hpp b/src/drivers/imu/bmi088/BMI088_accel.hpp new file mode 100644 index 000000000000..20e89ff0096d --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_accel.hpp @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "BMI088.hpp" + +#include +#include + +#define BMI088_DEVICE_PATH_ACCEL "/dev/bmi088_accel" +#define BMI088_DEVICE_PATH_ACCEL_EXT "/dev/bmi088_accel_ext" + +// BMI088 Accel registers +#define BMI088_ACC_CHIP_ID 0x00 + +#define BMI088_ACC_ERR_REG 0x02 +#define BMI088_ACC_STATUS 0x03 +#define BMI088_ACC_X_L 0x12 +#define BMI088_ACC_X_H 0x13 +#define BMI088_ACC_Y_L 0x14 +#define BMI088_ACC_Y_H 0x15 +#define BMI088_ACC_Z_L 0x16 +#define BMI088_ACC_Z_H 0x17 +#define BMI088_ACC_SENSORTIME_0 0x18 +#define BMI088_ACC_SENSORTIME_1 0x19 +#define BMI088_ACC_SENSORTIME_2 0x1A +#define BMI088_ACC_INT_STAT_1 0x1D +#define BMI088_ACC_TEMP_H 0x22 +#define BMI088_ACC_TEMP_L 0x23 +#define BMI088_ACC_CONF 0x40 +#define BMI088_ACC_RANGE 0x41 +#define BMI088_ACC_INT1_IO_CONF 0x53 +#define BMI088_ACC_INT2_IO_CONF 0x54 +#define BMI088_ACC_INT1_INT2_MAP_DATA 0x58 +#define BMI088_ACC_SELF_TEST 0x6D +#define BMI088_ACC_PWR_CONF 0x7C +#define BMI088_ACC_PWR_CTRL 0x7D +#define BMI088_ACC_SOFTRESET 0x7E + +// BMI088 Accelerometer Chip-Id +#define BMI088_ACC_WHO_AM_I 0x1E + +// BMI088_ACC_ERR_REG 0x02 +#define BMI088_ACC_ERR_REG_NO_ERROR (0x00<<2) +#define BMI088_ACC_ERR_REG_ERROR (0x01<<2) + +#define BMI088_ACC_ERR_REG_FATAL_ERROR (0x01<<0) + +// BMI088_ACC_STATUS 0x03 +#define BMI088_ACC_STATUS_DRDY (0x01<<7) + +// BMI088_ACC_INT_STAT_1 0x01D +#define BMI088_ACC_INT_STAT_1_DRDY (0x01<<7) + +// BMI088_ACC_CONF 0x40 +#define BMI088_ACC_CONF_BWP_4 (0x08<<4) +#define BMI088_ACC_CONF_BWP_2 (0x09<<4) +#define BMI088_ACC_CONF_BWP_NORMAL (0x0A<<4) + +#define BMI088_ACC_CONF_ODR_12_5 (0x05<<0) +#define BMI088_ACC_CONF_ODR_25 (0x06<<0) +#define BMI088_ACC_CONF_ODR_50 (0x07<<0) +#define BMI088_ACC_CONF_ODR_100 (0x08<<0) +#define BMI088_ACC_CONF_ODR_200 (0x09<<0) +#define BMI088_ACC_CONF_ODR_400 (0x0A<<0) +#define BMI088_ACC_CONF_ODR_800 (0x0B<<0) +#define BMI088_ACC_CONF_ODR_1600 (0x0C<<0) + +// BMI088_ACC_RANGE 0x41 +#define BMI088_ACCEL_RANGE_3_G (0x00<<0) +#define BMI088_ACCEL_RANGE_6_G (0x01<<0) +#define BMI088_ACCEL_RANGE_12_G (0x02<<0) +#define BMI088_ACCEL_RANGE_24_G (0x03<<0) + +// BMI088_ACC_INT1_IO_CONF 0x53 +#define BMI088_ACC_INT1_IO_CONF_INT1_IN (0x01<<4) + +#define BMI088_ACC_INT1_IO_CONF_INT1_OUT (0x01<<3) + +#define BMI088_ACC_INT1_IO_CONF_PP (0x00<<2) +#define BMI088_ACC_INT1_IO_CONF_OD (0x01<<2) + +#define BMI088_ACC_INT1_IO_CONF_ACTIVE_LOW (0x00<<1) +#define BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH (0x01<<1) + +// BMI088_ACC_INT2_IO_CONF 0x54 +#define BMI088_ACC_INT2_IO_CONF_INT1_IN (0x01<<4) + +#define BMI088_ACC_INT2_IO_CONF_INT1_OUT (0x01<<3) + +#define BMI088_ACC_INT2_IO_CONF_PP (0x00<<2) +#define BMI088_ACC_INT2_IO_CONF_OD (0x01<<2) + +#define BMI088_ACC_INT2_IO_CONF_ACTIVE_LOW (0x00<<1) +#define BMI088_ACC_INT2_IO_CONF_ACTIVE_HIGH (0x01<<1) + +// BMI088_ACC_INT1_INT2_MAP_DATA 0x54 +#define BMI088_ACC_INT1_INT2_MAP_DATA_INT2_DRDY (0x01<<6) +#define BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY (0x01<<2) + +// BMI088_ACC_SELF_TEST 0x6D +#define BMI088_ACC_SELF_TEST_OFF (0x00<<0) +#define BMI088_ACC_SELF_TEST_POSITIVE (0x0D<<0) +#define BMI088_ACC_SELF_TEST_NEGATIVE (0x09<<0) + +// BMI088_ACC_PWR_CONF 0x7C +#define BMI088_ACC_PWR_CONF_SUSPEND (0x03<<0) +#define BMI088_ACC_PWR_CONF_ACTIVE (0x00<<0) + +// BMI088_ACC_PWR_CTRL 0x7D +#define BMI088_ACC_PWR_CTRL_EN (0x04<<0) + + +/////// +// To Do check these defaults and maks below +/// + +// Default and Max values +#define BMI088_ACCEL_DEFAULT_RANGE_G 24 +#define BMI088_ACCEL_DEFAULT_RATE 800 +#define BMI088_ACCEL_MAX_RATE 800 +#define BMI088_ACCEL_MAX_PUBLISH_RATE 800 + +#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50 + +class BMI088_accel : public BMI088, public px4::ScheduledWorkItem +{ +public: + BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation); + virtual ~BMI088_accel(); + + virtual int init(); + + // Start automatic measurement. + void start(); + + // We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation + virtual uint8_t read_reg(unsigned reg); + + // We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation + virtual uint16_t read_reg16(unsigned reg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + PX4Accelerometer _px4_accel; + + perf_counter_t _sample_perf; + perf_counter_t _measure_interval; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _duplicates; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI088_ACCEL_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + + bool _got_duplicate; + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + void Run() override; + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI088_accel + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the BMI088_accel, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the BMI088_accel measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Set accel sample rate + */ + int accel_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI088_accel(const BMI088_accel &); + BMI088_accel operator=(const BMI088_accel &); + +}; diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp new file mode 100644 index 000000000000..806800369421 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * 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 "BMI088_gyro.hpp" +#include "BMI088_accel.hpp" + + +/* + * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. + * This is a ugly HACK! The driver should potentially be rewritten with the gyro as subdriver. + */ +__EXPORT float _accel_last_temperature_copy = 0; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ + +const uint8_t BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS] = { BMI088_GYR_CHIP_ID, + BMI088_GYR_LPM1, + BMI088_GYR_BW, + BMI088_GYR_RANGE, + BMI088_GYR_INT_EN_0, + BMI088_GYR_INT_EN_1, + BMI088_GYR_INT_MAP_1 + }; + +BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : + BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")), + _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_gyro_measure_interval")), + _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")) +{ + _px4_gyro.set_device_type(DRV_DEVTYPE_BMI088); +} + +BMI088_gyro::~BMI088_gyro() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_measure_interval); + perf_free(_bad_transfers); + perf_free(_bad_registers); +} + +int +BMI088_gyro::init() +{ + /* do SPI init (and probe) first */ + int ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + return reset(); +} + +int BMI088_gyro::reset() +{ + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);//Soft-reset + usleep(5000); + write_checked_reg(BMI088_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate()) + write_checked_reg(BMI088_GYR_RANGE, 0);// Write Gyro range + write_checked_reg(BMI088_GYR_INT_EN_0, BMI088_GYR_DRDY_INT_EN); //Enable DRDY interrupt + write_checked_reg(BMI088_GYR_INT_MAP_1, BMI088_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 + + set_gyro_range(BMI088_GYRO_DEFAULT_RANGE_DPS);// set Gyro range + gyro_set_sample_rate(BMI088_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth + + //Enable Gyroscope in normal mode + write_reg(BMI088_GYR_LPM1, BMI088_GYRO_NORMAL); + up_udelay(1000); + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + return OK; +} + +int +BMI088_gyro::probe() +{ + /* look for device ID */ + _whoami = read_reg(BMI088_GYR_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI088_GYR_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + printf("unexpected whoami 0x%02x\n", _whoami); + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + return -EIO; +} + +int +BMI088_gyro::gyro_set_sample_rate(float frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_GYRO_BW_MASK; + + if (frequency <= 100) { + setbits |= BMI088_GYRO_RATE_100; /* 32 Hz cutoff */ + //_gyro_sample_rate = 100; + + } else if (frequency <= 250) { + setbits |= BMI088_GYRO_RATE_400; /* 47 Hz cutoff */ + //_gyro_sample_rate = 400; + + } else if (frequency <= 1000) { + setbits |= BMI088_GYRO_RATE_1000; /* 116 Hz cutoff */ + //_gyro_sample_rate = 1000; + + } else if (frequency > 1000) { + setbits |= BMI088_GYRO_RATE_2000; /* 230 Hz cutoff */ + //_gyro_sample_rate = 2000; + + } else { + return -EINVAL; + } + + modify_reg(BMI088_GYR_BW, clearbits, setbits); + + return OK; +} + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI088_gyro::test_error() +{ + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + +void +BMI088_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI088_gyro::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + +int +BMI088_gyro::set_gyro_range(unsigned max_dps) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_GYRO_RANGE_125_DPS | BMI088_GYRO_RANGE_250_DPS; + float lsb_per_dps; + + if (max_dps == 0) { + max_dps = 2000; + } + + if (max_dps <= 125) { + //max_gyro_dps = 125; + lsb_per_dps = 262.4; + setbits |= BMI088_GYRO_RANGE_125_DPS; + + } else if (max_dps <= 250) { + //max_gyro_dps = 250; + lsb_per_dps = 131.2; + setbits |= BMI088_GYRO_RANGE_250_DPS; + + } else if (max_dps <= 500) { + //max_gyro_dps = 500; + lsb_per_dps = 65.6; + setbits |= BMI088_GYRO_RANGE_500_DPS; + + } else if (max_dps <= 1000) { + //max_gyro_dps = 1000; + lsb_per_dps = 32.8; + setbits |= BMI088_GYRO_RANGE_1000_DPS; + + } else if (max_dps <= 2000) { + //max_gyro_dps = 2000; + lsb_per_dps = 16.4; + setbits |= BMI088_GYRO_RANGE_2000_DPS; + + } else { + return -EINVAL; + } + + _px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps)); + + modify_reg(BMI088_GYR_RANGE, clearbits, setbits); + + return OK; +} + +void +BMI088_gyro::start() +{ + /* make sure we are stopped first */ + stop(); + + /* start polling at the specified rate */ + ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000); +} + +void +BMI088_gyro::stop() +{ + ScheduleClear(); +} + +void +BMI088_gyro::Run() +{ + /* make another measurement */ + measure(); +} + +void +BMI088_gyro::measure_trampoline(void *arg) +{ + BMI088_gyro *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +BMI088_gyro::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI088_GYRO_NUM_CHECKED_REGISTERS; +} + +void +BMI088_gyro::measure() +{ + perf_count(_measure_interval); + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct BMI_GyroReport bmi_gyroreport; + + struct Report { + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the BMI088 gyro in one pass. + */ + bmi_gyroreport.cmd = BMI088_GYR_X_L | DIR_READ; + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) { + return; + } + + check_registers(); + + + // Get the last temperature from the accelerometer (the Gyro does not have its own temperature measurement) + _last_temperature = _accel_last_temperature_copy; + + report.gyro_x = bmi_gyroreport.gyro_x; + report.gyro_y = bmi_gyroreport.gyro_y; + report.gyro_z = bmi_gyroreport.gyro_z; + + if (report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably an SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi088 does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again, but don't return any data yet + _register_wait--; + return; + } + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_gyro.set_error_count(error_count); + + // Get the temperature from the accelerometer part of the BMI088, because the gyro part does not have a temperature register + _px4_gyro.set_temperature(_accel_last_temperature_copy); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +BMI088_gyro::print_info() +{ + PX4_INFO("Gyro"); + + perf_print_counter(_sample_perf); + perf_print_counter(_measure_interval); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + _px4_gyro.print_status(); +} + +void +BMI088_gyro::print_registers() +{ + uint8_t index = 0; + printf("BMI088 gyro registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + + printf("\n"); +} diff --git a/src/drivers/imu/bmi088/BMI088_gyro.hpp b/src/drivers/imu/bmi088/BMI088_gyro.hpp new file mode 100644 index 000000000000..7b7c73b6224c --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_gyro.hpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "BMI088.hpp" + +#define BMI088_DEVICE_PATH_GYRO "/dev/bmi088_gyro" +#define BMI088_DEVICE_PATH_GYRO_EXT "/dev/bmi088_gyro_ext" + +// BMI088 Gyro registers +#define BMI088_GYR_CHIP_ID 0x00 +#define BMI088_GYR_X_L 0x02 +#define BMI088_GYR_X_H 0x03 +#define BMI088_GYR_Y_L 0x04 +#define BMI088_GYR_Y_H 0x05 +#define BMI088_GYR_Z_L 0x06 +#define BMI088_GYR_Z_H 0x07 +#define BMI088_GYR_INT_STATUS_0 0x09 +#define BMI088_GYR_INT_STATUS_1 0x0A +#define BMI088_GYR_INT_STATUS_2 0x0B +#define BMI088_GYR_INT_STATUS_3 0x0C +#define BMI088_GYR_FIFO_STATUS 0x0E +#define BMI088_GYR_RANGE 0x0F +#define BMI088_GYR_BW 0x10 +#define BMI088_GYR_LPM1 0x11 +#define BMI088_GYR_LPM2 0x12 +#define BMI088_GYR_RATE_HBW 0x13 +#define BMI088_GYR_SOFTRESET 0x14 +#define BMI088_GYR_INT_EN_0 0x15 +#define BMI088_GYR_INT_EN_1 0x16 +#define BMI088_GYR_INT_MAP_0 0x17 +#define BMI088_GYR_INT_MAP_1 0x18 +#define BMI088_GYR_INT_MAP_2 0x19 +#define BMI088_GYRO_0_REG 0x1A +#define BMI088_GYRO_1_REG 0x1B +#define BMI088_GYRO_2_REG 0x1C +#define BMI088_GYRO_3_REG 0x1E +#define BMI088_GYR_INT_LATCH 0x21 +#define BMI088_GYR_INT_LH_0 0x22 +#define BMI088_GYR_INT_LH_1 0x23 +#define BMI088_GYR_INT_LH_2 0x24 +#define BMI088_GYR_INT_LH_3 0x25 +#define BMI088_GYR_INT_LH_4 0x26 +#define BMI088_GYR_INT_LH_5 0x27 +#define BMI088_GYR_SOC 0x31 +#define BMI088_GYR_A_FOC 0x32 +#define BMI088_GYR_TRIM_NVM_CTRL 0x33 +#define BMI088_BGW_SPI3_WDT 0x34 +#define BMI088_GYR_OFFSET_COMP 0x36 +#define BMI088_GYR_OFFSET_COMP_X 0x37 +#define BMI088_GYR_OFFSET_COMP_Y 0x38 +#define BMI088_GYR_OFFSET_COMP_Z 0x39 +#define BMI088_GYR_TRIM_GPO 0x3A +#define BMI088_GYR_TRIM_GP1 0x3B +#define BMI088_GYR_SELF_TEST 0x3C +#define BMI088_GYR_FIFO_CONFIG_0 0x3D +#define BMI088_GYR_FIFO_CONFIG_1 0x3E +#define BMI088_GYR_FIFO_DATA 0x3F + +// BMI088 Gyroscope Chip-Id +#define BMI088_GYR_WHO_AM_I 0x0F + +//ODR & DLPF filter bandwidth settings (they are coupled) +#define BMI088_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0) + +//BMI088_GYR_LPM1 0x11 +#define BMI088_GYRO_NORMAL (0<<7) | (0<<5) +#define BMI088_GYRO_DEEP_SUSPEND (0<<7) | (1<<5) +#define BMI088_GYRO_SUSPEND (1<<7) | (0<<5) + +//BMI088_GYR_RANGE 0x0F +#define BMI088_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0) +#define BMI088_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0) +#define BMI088_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0) + +//BMI088_GYR_INT_EN_0 0x15 +#define BMI088_GYR_DRDY_INT_EN (1<<7) + +//BMI088_GYR_INT_MAP_1 0x18 +#define BMI088_GYR_DRDY_INT1 (1<<0) + +// Default and Max values +#define BMI088_GYRO_DEFAULT_RANGE_DPS 2000 +#define BMI088_GYRO_DEFAULT_RATE 1000 +#define BMI088_GYRO_MAX_RATE 1000 +#define BMI088_GYRO_MAX_PUBLISH_RATE 280 + +#define BMI088_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50 + +/* Mask definitions for Gyro bandwidth */ +#define BMI088_GYRO_BW_MASK 0x0F + +class BMI088_gyro : public BMI088, public px4::ScheduledWorkItem +{ +public: + BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation); + virtual ~BMI088_gyro(); + + virtual int init(); + + // Start automatic measurement. + void start(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + PX4Gyroscope _px4_gyro; + + perf_counter_t _sample_perf; + perf_counter_t _measure_interval; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI088_GYRO_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + + // last temperature reading for print_info() + float _last_temperature; + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + void Run() override; + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI088_gyro + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the BMI088_gyro, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the BMI088_gyro measurement range. + * + * @param max_dps The maximum DPS value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_gyro_range(unsigned max_dps); + + /* + * set gyro sample rate + */ + int gyro_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI088_gyro(const BMI088_gyro &); + BMI088_gyro operator=(const BMI088_gyro &); + +#pragma pack(push, 1) + /** + * Report conversation within the BMI088_gyro, including command byte and + * interrupt status. + */ + struct BMI_GyroReport { + uint8_t cmd; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + }; +#pragma pack(pop) + +}; diff --git a/src/drivers/lights/oreoled/CMakeLists.txt b/src/drivers/imu/bmi088/CMakeLists.txt similarity index 92% rename from src/drivers/lights/oreoled/CMakeLists.txt rename to src/drivers/imu/bmi088/CMakeLists.txt index 33fdafbc02ac..35a92c3cfe4c 100644 --- a/src/drivers/lights/oreoled/CMakeLists.txt +++ b/src/drivers/imu/bmi088/CMakeLists.txt @@ -31,12 +31,12 @@ # ############################################################################ px4_add_module( - MODULE drivers__oreoled - MAIN oreoled - STACK_MAIN 1024 + MODULE drivers__bmi88 + MAIN bmi088 COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable SRCS - oreoled.cpp - DEPENDS + BMI088_accel.cpp + BMI088_gyro.cpp + bmi088_main.cpp ) - diff --git a/src/drivers/imu/bmi088/bmi088_main.cpp b/src/drivers/imu/bmi088/bmi088_main.cpp new file mode 100644 index 000000000000..fe4e1a74ee41 --- /dev/null +++ b/src/drivers/imu/bmi088/bmi088_main.cpp @@ -0,0 +1,413 @@ +/**************************************************************************** + * + * 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 "BMI088_accel.hpp" +#include "BMI088_gyro.hpp" + +/** driver 'main' command */ +extern "C" { __EXPORT int bmi088_main(int argc, char *argv[]); } + +enum sensor_type { + BMI088_NONE = 0, + BMI088_ACCEL = 1, + BMI088_GYRO +}; + +namespace bmi088 +{ + +BMI088_accel *g_acc_dev_int; // on internal bus (accel) +BMI088_accel *g_acc_dev_ext; // on external bus (accel) +BMI088_gyro *g_gyr_dev_int; // on internal bus (gyro) +BMI088_gyro *g_gyr_dev_ext; // on external bus (gyro) + +void start(bool, enum Rotation, enum sensor_type); +void stop(bool, enum sensor_type); +void info(bool, enum sensor_type); +void regdump(bool, enum sensor_type); +void testerror(bool, enum sensor_type); +void usage(); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation, enum sensor_type sensor) +{ + + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + const char *path_accel = external_bus ? BMI088_DEVICE_PATH_ACCEL_EXT : BMI088_DEVICE_PATH_ACCEL; + + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + const char *path_gyro = external_bus ? BMI088_DEVICE_PATH_GYRO_EXT : BMI088_DEVICE_PATH_GYRO; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr != nullptr) + /* if already started, the still command succeeded */ + { + errx(0, "bmi088 accel sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_EXT, path_accel, PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_SENSORS3, path_accel, PX4_SPIDEV_BMI088_ACC, rotation); + } + + if (*g_dev_acc_ptr == nullptr) { + goto fail_accel; + } + + if (OK != (*g_dev_acc_ptr)->init()) { + goto fail_accel; + } + + // start automatic data collection + (*g_dev_acc_ptr)->start(); + } + + if (sensor == BMI088_GYRO) { + + if (*g_dev_gyr_ptr != nullptr) { + errx(0, "bmi088 gyro sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_ptr = new BMI088_gyro(PX4_SPI_BUS_EXT, path_gyro, PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_gyr_ptr = new BMI088_gyro(PX4_SPI_BUS_SENSORS3, path_gyro, PX4_SPIDEV_BMI088_GYR, rotation); + } + + if (*g_dev_gyr_ptr == nullptr) { + goto fail_gyro; + } + + if (OK != (*g_dev_gyr_ptr)->init()) { + goto fail_gyro; + } + + // start automatic data collection + (*g_dev_gyr_ptr)->start(); + } + + exit(PX4_OK); + +fail_accel: + + if (*g_dev_acc_ptr != nullptr) { + delete (*g_dev_acc_ptr); + *g_dev_acc_ptr = nullptr; + } + + PX4_WARN("No BMI088 accel found"); + exit(PX4_ERROR); + +fail_gyro: + + if (*g_dev_gyr_ptr != nullptr) { + delete (*g_dev_gyr_ptr); + *g_dev_gyr_ptr = nullptr; + } + + PX4_WARN("No BMI088 gyro found"); + exit(PX4_ERROR); +} + +void +stop(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr != nullptr) { + delete *g_dev_acc_ptr; + *g_dev_acc_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi088 accel sensor already stopped."); + } + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr != nullptr) { + delete *g_dev_gyr_ptr; + *g_dev_gyr_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi088 gyro sensor already stopped."); + } + } + + exit(0); + +} + +/** + * Print a little info about the driver. + */ +void +info(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + printf("state @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_info(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + printf("state @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_info(); + } + + exit(0); +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + printf("regdump @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_registers(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + printf("regdump @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_registers(); + } + + exit(0); +} + +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + (*g_dev_acc_ptr)->test_error(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + (*g_dev_gyr_ptr)->test_error(); + } + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'"); + warnx("options:"); + warnx(" -X (external bus)"); + warnx(" -R rotation"); + warnx(" -A (Enable Accelerometer)"); + warnx(" -G (Enable Gyroscope)"); +} + +}//namespace ends + + +BMI088::BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, + uint32_t frequency, enum Rotation rotation): + SPI(name, devname, bus, device, mode, frequency), + _whoami(0), + _register_wait(0), + _reset_wait(0), + _rotation(rotation), + _checked_next(0) +{ +} + +uint8_t +BMI088::read_reg(unsigned reg) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +BMI088::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +BMI088::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +int +bmi088_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + enum sensor_type sensor = BMI088_NONE; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "XR:AG", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'A': + sensor = BMI088_ACCEL; + break; + + case 'G': + sensor = BMI088_GYRO; + break; + + default: + bmi088::usage(); + exit(0); + } + } + + const char *verb = argv[myoptind]; + + if (sensor == BMI088_NONE) { + bmi088::usage(); + exit(0); + } + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmi088::start(external_bus, rotation, sensor); + } + + /* + * Stop the driver. + */ + if (!strcmp(verb, "stop")) { + bmi088::stop(external_bus, sensor); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmi088::info(external_bus, sensor); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + bmi088::regdump(external_bus, sensor); + } + + if (!strcmp(verb, "testerror")) { + bmi088::testerror(external_bus, sensor); + } + + bmi088::usage(); + exit(1); +} diff --git a/src/drivers/imu/bmi160/CMakeLists.txt b/src/drivers/imu/bmi160/CMakeLists.txt index 19f5dfc3d40c..343775a3b525 100644 --- a/src/drivers/imu/bmi160/CMakeLists.txt +++ b/src/drivers/imu/bmi160/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__bmi160 MAIN bmi160 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index cad0827a2c4b..473ecc99239f 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #define DIR_READ 0x80 diff --git a/src/drivers/imu/fxas21002c/CMakeLists.txt b/src/drivers/imu/fxas21002c/CMakeLists.txt index d5478993684f..ea04e45239d1 100644 --- a/src/drivers/imu/fxas21002c/CMakeLists.txt +++ b/src/drivers/imu/fxas21002c/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__fxas21002c MAIN fxas21002c - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index bce8b6bdaf71..08aa80aa42e8 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -159,10 +159,7 @@ /* default values for this device */ #define FXAS21002C_MAX_RATE 800 #define FXAS21002C_DEFAULT_RATE FXAS21002C_MAX_RATE -#define FXAS21002C_MAX_OUTPUT_RATE 280 #define FXAS21002C_DEFAULT_RANGE_DPS 2000 -#define FXAS21002C_DEFAULT_FILTER_FREQ 30 -#define FXAS21002C_TEMP_OFFSET_CELSIUS 40 #define FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ 64 // ODR dependant /* @@ -193,10 +190,11 @@ FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) : SPI("FXAS21002C", nullptr, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000), ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, "fxas21002c_acc_read")), - _errors(perf_alloc(PC_COUNT, "fxas21002c_err")), - _bad_registers(perf_alloc(PC_COUNT, "fxas21002c_bad_reg")), - _duplicates(perf_alloc(PC_COUNT, "fxas21002c_acc_dupe")) + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _sample_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": read interval")), + _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), + _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicate reading")) { _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_FXAS2100C); } @@ -208,6 +206,7 @@ FXAS21002C::~FXAS21002C() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_sample_interval_perf); perf_free(_errors); perf_free(_bad_registers); perf_free(_duplicates); @@ -529,8 +528,11 @@ FXAS21002C::check_registers(void) void FXAS21002C::measure() { - /* status register and data as read back from the device */ + // start the performance counter + perf_begin(_sample_perf); + perf_count(_sample_interval_perf); + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { uint8_t cmd; @@ -541,9 +543,6 @@ FXAS21002C::measure() } raw_gyro_report{}; #pragma pack(pop) - /* start the performance counter */ - perf_begin(_sample_perf); - check_registers(); if (_register_wait != 0) { @@ -599,6 +598,7 @@ FXAS21002C::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); + perf_print_counter(_sample_interval_perf); perf_print_counter(_errors); perf_print_counter(_bad_registers); perf_print_counter(_duplicates); diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp index 997e969b08b3..da7496596d36 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem @@ -81,6 +81,7 @@ class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem unsigned _read{0}; perf_counter_t _sample_perf; + perf_counter_t _sample_interval_perf; perf_counter_t _errors; perf_counter_t _bad_registers; perf_counter_t _duplicates; diff --git a/src/drivers/imu/fxos8701cq/CMakeLists.txt b/src/drivers/imu/fxos8701cq/CMakeLists.txt index 209615dacc00..7ec14b5f46d9 100644 --- a/src/drivers/imu/fxos8701cq/CMakeLists.txt +++ b/src/drivers/imu/fxos8701cq/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,11 +31,12 @@ # ############################################################################ px4_add_module( - MODULE drivers__fxos8701cq + MODULE drivers__imu__fxos8701cq MAIN fxos8701cq - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS - fxos8701cq.cpp + FXOS8701CQ.cpp + FXOS8701CQ.hpp + fxos8701cq_main.cpp ) diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp new file mode 100644 index 000000000000..c5ecc1c1c78e --- /dev/null +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -0,0 +1,468 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 fxos8701cq.cpp + * Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and + * magnetometer connected via SPI. + */ + +#include "FXOS8701CQ.hpp" + +using namespace time_literals; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = { + FXOS8701CQ_WHOAMI, + FXOS8701CQ_XYZ_DATA_CFG, + FXOS8701CQ_CTRL_REG1, + FXOS8701CQ_M_CTRL_REG1, + FXOS8701CQ_M_CTRL_REG2, +}; + +FXOS8701CQ::FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation) : + SPI("FXOS8701CQ", nullptr, bus, device, SPIDEV_MODE0, 1 * 1000 * 1000), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), ORB_PRIO_LOW, rotation), +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), + _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")), +#endif + _accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")), + _accel_sample_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": acc read interval")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad reg")), + _accel_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": acc dupe")) +{ + set_device_type(DRV_ACC_DEVTYPE_FXOS8701C); + + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_FXOS8701C); + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + _px4_mag.set_device_type(DRV_ACC_DEVTYPE_FXOS8701C); + _px4_mag.set_scale(0.001f); +#endif +} + +FXOS8701CQ::~FXOS8701CQ() +{ + // make sure we are truly inactive + stop(); + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + perf_free(_mag_sample_perf); +#endif + + // delete the perf counter + perf_free(_accel_sample_perf); + perf_free(_accel_sample_interval_perf); + perf_free(_bad_registers); + perf_free(_accel_duplicates); +} + +int +FXOS8701CQ::init() +{ + // do SPI init (and probe) first + int ret = SPI::init(); + + if (ret != OK) { + PX4_ERR("SPI init failed"); + return ret; + } + + reset(); + + start(); + + return PX4_OK; +} + +void +FXOS8701CQ::reset() +{ + // enable accel set it To Standby + write_checked_reg(FXOS8701CQ_CTRL_REG1, 0); + write_checked_reg(FXOS8701CQ_XYZ_DATA_CFG, 0); + + // Use hybird mode to read Accel and Mag + write_checked_reg(FXOS8701CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); + + // Use the hybird auto increment mode to read all the data at the same time + write_checked_reg(FXOS8701CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC); + + accel_set_range(FXOS8701C_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(FXOS8701C_ACCEL_DEFAULT_RATE); + + // enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode + write_checked_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE); +} + +int +FXOS8701CQ::probe() +{ + // verify that the device is attached and functioning + uint8_t whoami = read_reg(FXOS8701CQ_WHOAMI); + bool success = (whoami == FXOS8700CQ_WHOAMI_VAL) || (whoami == FXOS8701CQ_WHOAMI_VAL); + + if (success) { + _checked_values[0] = whoami; + return OK; + } + + return -EIO; +} + +uint8_t +FXOS8701CQ::read_reg(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_READ(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = 0; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[2]; +} + +void +FXOS8701CQ::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_WRITE(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +FXOS8701CQ::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + +void +FXOS8701CQ::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +int +FXOS8701CQ::accel_set_range(unsigned max_g) +{ + uint8_t setbits = 0; + float lsb_per_g; + + if (max_g == 0 || max_g > 8) { + max_g = 8; + } + + if (max_g > 4) { // 8g + setbits = XYZ_DATA_CFG_FS_8G; + lsb_per_g = 1024; + //max_accel_g = 8; + + } else if (max_g > 2) { // 4g + setbits = XYZ_DATA_CFG_FS_4G; + lsb_per_g = 2048; + //max_accel_g = 4; + + } else { // 2g + setbits = XYZ_DATA_CFG_FS_2G; + lsb_per_g = 4096; + //max_accel_g = 2; + } + + float accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); + + modify_reg(FXOS8701CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits); + + _px4_accel.set_scale(accel_range_scale); + + return OK; +} + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) +int +FXOS8701CQ::mag_set_range(unsigned max_ga) +{ + // mag_range_ga = 12; + float mag_range_scale = 0.001f; + + _px4_mag.set_scale(mag_range_scale); + + return OK; +} +#endif + +int +FXOS8701CQ::accel_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + + // The selected ODR is reduced by a factor of two when the device is operated in hybrid mode. + uint8_t active = read_reg(FXOS8701CQ_CTRL_REG1) & CTRL_REG1_ACTIVE; + + if (frequency == 0) { + frequency = FXOS8701C_ACCEL_DEFAULT_RATE; + } + + if (frequency <= 25) { + setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2 + _accel_samplerate = 25; + + } else if (frequency <= 50) { + setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2 + _accel_samplerate = 50; + + } else if (frequency <= 100) { + setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2 + _accel_samplerate = 100; + + } else if (frequency <= 200) { + setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2; + _accel_samplerate = 200; + + } else if (frequency <= 400) { + setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2; + _accel_samplerate = 400; + + } else { + return -EINVAL; + } + + modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0); + modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits); + modify_reg(FXOS8701CQ_CTRL_REG1, 0, active); + + return OK; +} + +void +FXOS8701CQ::start() +{ + // make sure we are stopped first + stop(); + + // start polling at the specified rate + ScheduleOnInterval(1000000 / (FXOS8701C_ACCEL_DEFAULT_RATE) - FXOS8701C_TIMER_REDUCTION, 10000); +} + +void +FXOS8701CQ::stop() +{ + ScheduleClear(); +} + +void +FXOS8701CQ::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % FXOS8701C_NUM_CHECKED_REGISTERS; +} + +void +FXOS8701CQ::Run() +{ + // start the performance counter + perf_begin(_accel_sample_perf); + perf_count(_accel_sample_interval_perf); + + // status register and data as read back from the device +#pragma pack(push, 1) + struct { + uint8_t cmd[2]; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + int16_t mx; + int16_t my; + int16_t mz; + } raw_accel_mag_report{}; +#pragma pack(pop) + + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + + /* fetch data from the sensor */ + raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8701CQ_DR_STATUS); + raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8701CQ_DR_STATUS); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); + + if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) { + perf_end(_accel_sample_perf); + perf_count(_accel_duplicates); + return; + } + + /* + * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. + * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor + * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the + * temperature sensor is uncalibrated and its output for a given temperature will vary from + * one device to the next + */ + float temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f; + + _px4_accel.set_temperature(temperature); + + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + _px4_accel.set_error_count(perf_event_count(_bad_registers)); + + int16_t x = swap16RightJustify14(raw_accel_mag_report.x); + int16_t y = swap16RightJustify14(raw_accel_mag_report.y); + int16_t z = swap16RightJustify14(raw_accel_mag_report.z); + _px4_accel.update(timestamp_sample, x, y, z); + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + + if (hrt_elapsed_time(&_mag_last_measure) >= 10_ms) { + int16_t mag_x = swap16(raw_accel_mag_report.mx); + int16_t mag_y = swap16(raw_accel_mag_report.my); + int16_t mag_z = swap16(raw_accel_mag_report.mz); + _px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z); + } + +#endif + + // stop the perf counter + perf_end(_accel_sample_perf); +} + +void +FXOS8701CQ::print_info() +{ + perf_print_counter(_accel_sample_perf); + perf_print_counter(_accel_sample_interval_perf); + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + perf_print_counter(_mag_sample_perf); +#endif + perf_print_counter(_bad_registers); + perf_print_counter(_accel_duplicates); + + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } +} + +void +FXOS8701CQ::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + DEF_REG(FXOS8701CQ_DR_STATUS), + DEF_REG(FXOS8701CQ_OUT_X_MSB), + DEF_REG(FXOS8701CQ_XYZ_DATA_CFG), + DEF_REG(FXOS8701CQ_WHOAMI), + DEF_REG(FXOS8701CQ_CTRL_REG1), + DEF_REG(FXOS8701CQ_CTRL_REG2), + DEF_REG(FXOS8701CQ_M_DR_STATUS), + DEF_REG(FXOS8701CQ_M_OUT_X_MSB), + DEF_REG(FXOS8701CQ_M_CTRL_REG1), + DEF_REG(FXOS8701CQ_M_CTRL_REG2), + DEF_REG(FXOS8701CQ_M_CTRL_REG3), + }; + + for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) { + printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); + } +} + +void +FXOS8701CQ::test_error() +{ + // trigger an error + write_reg(FXOS8701CQ_CTRL_REG1, 0); +} diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp new file mode 100644 index 000000000000..26bcbbe232cb --- /dev/null +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 FXOS8701CQ.hpp + * Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and + * magnetometer connected via SPI. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) +#include +#endif + +/* SPI protocol address bits */ +#define DIR_READ(a) ((a) & 0x7f) +#define DIR_WRITE(a) ((a) | (1 << 7)) +#define ADDR_7(a) ((a) & (1 << 7)) +#define swap16(w) __builtin_bswap16((w)) +#define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2) + +#define FXOS8701CQ_DR_STATUS 0x00 +# define DR_STATUS_ZYXDR (1 << 3) + +#define FXOS8701CQ_OUT_X_MSB 0x01 + +#define FXOS8701CQ_XYZ_DATA_CFG 0x0e +# define XYZ_DATA_CFG_FS_SHIFTS 0 +# define XYZ_DATA_CFG_FS_MASK (3 << XYZ_DATA_CFG_FS_SHIFTS) +# define XYZ_DATA_CFG_FS_2G (0 << XYZ_DATA_CFG_FS_SHIFTS) +# define XYZ_DATA_CFG_FS_4G (1 << XYZ_DATA_CFG_FS_SHIFTS) +# define XYZ_DATA_CFG_FS_8G (2 << XYZ_DATA_CFG_FS_SHIFTS) + +#define FXOS8701CQ_WHOAMI 0x0d +# define FXOS8700CQ_WHOAMI_VAL 0xC7 +# define FXOS8701CQ_WHOAMI_VAL 0xCA + +#define FXOS8701CQ_CTRL_REG1 0x2a +# define CTRL_REG1_ACTIVE (1 << 0) +# define CTRL_REG1_DR_SHIFTS 3 +# define CTRL_REG1_DR_MASK (7 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR(n) (((n) & 7) << CTRL_REG1_DR_SHIFTS) +#define FXOS8701CQ_CTRL_REG2 0x2b +# define CTRL_REG2_AUTO_INC (1 << 5) + +#define FXOS8701CQ_M_DR_STATUS 0x32 +# define M_DR_STATUS_ZYXDR (1 << 3) +#define FXOS8701CQ_M_OUT_X_MSB 0x33 +#define FXOS8701CQ_TEMP 0x51 +#define FXOS8701CQ_M_CTRL_REG1 0x5b +# define M_CTRL_REG1_HMS_SHIFTS 0 +# define M_CTRL_REG1_HMS_MASK (3 << M_CTRL_REG1_HMS_SHIFTS) +# define M_CTRL_REG1_HMS_A (0 << M_CTRL_REG1_HMS_SHIFTS) +# define M_CTRL_REG1_HMS_M (1 << M_CTRL_REG1_HMS_SHIFTS) +# define M_CTRL_REG1_HMS_AM (3 << M_CTRL_REG1_HMS_SHIFTS) +# define M_CTRL_REG1_OS_SHIFTS 2 +# define M_CTRL_REG1_OS_MASK (7 << M_CTRL_REG1_HMS_SHIFTS) +# define M_CTRL_REG1_OS(n) (((n) & 7) << M_CTRL_REG1_OS_SHIFTS) + +#define FXOS8701CQ_M_CTRL_REG2 0x5c +#define FXOS8701CQ_M_CTRL_REG3 0x5d + +#define DEF_REG(r) {r, #r} + +/* default values for this device */ +#define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8 +#define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */ + +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define FXOS8701C_TIMER_REDUCTION 240 + +class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem +{ +public: + FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation); + virtual ~FXOS8701CQ(); + + virtual int init(); + + void print_info(); + void print_registers(); + void test_error(); + +protected: + virtual int probe(); + +private: + + void Run() override; + + void start(); + void stop(); + void reset(); + + /** + * check key registers for correct values + */ + void check_registers(); + + /** + * Read a register from the FXOS8701C + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the FXOS8701C + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the FXOS8701C + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the FXOS8701C, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the FXOS8701C accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_range(unsigned max_g); + + /** + * Set the FXOS8701C mag measurement range. + * + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the FXOS8701C internal accel and mag sampling frequency. + * + * @param frequency The internal accel and mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int accel_set_samplerate(unsigned frequency); + + + PX4Accelerometer _px4_accel; + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) + PX4Magnetometer _px4_mag; + hrt_abstime _mag_last_measure{0}; + perf_counter_t _mag_sample_perf; +#endif + + unsigned _accel_samplerate{FXOS8701C_ACCEL_DEFAULT_RATE}; + + perf_counter_t _accel_sample_perf; + perf_counter_t _accel_sample_interval_perf; + perf_counter_t _bad_registers; + perf_counter_t _accel_duplicates; + + uint8_t _register_wait{0}; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset + static constexpr int FXOS8701C_NUM_CHECKED_REGISTERS{5}; + static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS] {}; + uint8_t _checked_next{0}; + +}; diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp deleted file mode 100644 index 0d693db3eea2..000000000000 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ /dev/null @@ -1,1923 +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 fxos8701cq.cpp - * Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and - * magnetometer connected via SPI. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -# include -#endif - -/* SPI protocol address bits */ -#define DIR_READ(a) ((a) & 0x7f) -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define ADDR_7(a) ((a) & (1 << 7)) -#define swap16(w) __builtin_bswap16((w)) -#define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2) -#define FXOS8701C_DEVICE_PATH_ACCEL "/dev/fxos8701cq_accel" -#define FXOS8701C_DEVICE_PATH_ACCEL_EXT "/dev/fxos8701cq_accel_ext" -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -# define FXOS8701C_DEVICE_PATH_MAG "/dev/fxos8701cq_mag" -#endif - -#define FXOS8701CQ_DR_STATUS 0x00 -# define DR_STATUS_ZYXDR (1 << 3) - -#define FXOS8701CQ_OUT_X_MSB 0x01 - -#define FXOS8701CQ_XYZ_DATA_CFG 0x0e -# define XYZ_DATA_CFG_FS_SHIFTS 0 -# define XYZ_DATA_CFG_FS_MASK (3 << XYZ_DATA_CFG_FS_SHIFTS) -# define XYZ_DATA_CFG_FS_2G (0 << XYZ_DATA_CFG_FS_SHIFTS) -# define XYZ_DATA_CFG_FS_4G (1 << XYZ_DATA_CFG_FS_SHIFTS) -# define XYZ_DATA_CFG_FS_8G (2 << XYZ_DATA_CFG_FS_SHIFTS) - -#define FXOS8701CQ_WHOAMI 0x0d -# define FXOS8700CQ_WHOAMI_VAL 0xC7 -# define FXOS8701CQ_WHOAMI_VAL 0xCA - -#define FXOS8701CQ_CTRL_REG1 0x2a -# define CTRL_REG1_ACTIVE (1 << 0) -# define CTRL_REG1_DR_SHIFTS 3 -# define CTRL_REG1_DR_MASK (7 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR(n) (((n) & 7) << CTRL_REG1_DR_SHIFTS) -#define FXOS8701CQ_CTRL_REG2 0x2b -# define CTRL_REG2_AUTO_INC (1 << 5) - -#define FXOS8701CQ_M_DR_STATUS 0x32 -# define M_DR_STATUS_ZYXDR (1 << 3) -#define FXOS8701CQ_M_OUT_X_MSB 0x33 -#define FXOS8701CQ_TEMP 0x51 -#define FXOS8701CQ_M_CTRL_REG1 0x5b -# define M_CTRL_REG1_HMS_SHIFTS 0 -# define M_CTRL_REG1_HMS_MASK (3 << M_CTRL_REG1_HMS_SHIFTS) -# define M_CTRL_REG1_HMS_A (0 << M_CTRL_REG1_HMS_SHIFTS) -# define M_CTRL_REG1_HMS_M (1 << M_CTRL_REG1_HMS_SHIFTS) -# define M_CTRL_REG1_HMS_AM (3 << M_CTRL_REG1_HMS_SHIFTS) -# define M_CTRL_REG1_OS_SHIFTS 2 -# define M_CTRL_REG1_OS_MASK (7 << M_CTRL_REG1_HMS_SHIFTS) -# define M_CTRL_REG1_OS(n) (((n) & 7) << M_CTRL_REG1_OS_SHIFTS) - -#define FXOS8701CQ_M_CTRL_REG2 0x5c -#define FXOS8701CQ_M_CTRL_REG3 0x5d - -#define DEF_REG(r) {r, #r} - -/* default values for this device */ -#define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8 -#define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */ -#define FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 -#define FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define FXOS8701C_ACCEL_MAX_OUTPUT_RATE 280 - -#define FXOS8701C_MAG_DEFAULT_RANGE_GA 12 /* It is fixed at 12 G */ -#define FXOS8701C_MAG_DEFAULT_RATE 100 - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates using the data ready bit. - This time reduction is enough to cope with worst case timing jitter - due to other timers - */ -#define FXOS8701C_TIMER_REDUCTION 240 - -extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); } - - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -class FXOS8701CQ_mag; -#endif - -class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem -{ -public: - FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation); - virtual ~FXOS8701CQ(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - /** - * dump register values - */ - void print_registers(); - - /** - * deliberately trigger an error - */ - void test_error(); - -protected: - virtual int probe(); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - friend class FXOS8701CQ_mag; - - virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); -#endif - -private: - - void Run() override; - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - FXOS8701CQ_mag *_mag; - unsigned _call_mag_interval; - ringbuffer::RingBuffer *_mag_reports; - - struct mag_calibration_s _mag_scale; - unsigned _mag_range_ga; - float _mag_range_scale; - unsigned _mag_samplerate; - unsigned _mag_read; - perf_counter_t _mag_sample_perf; - int16_t _last_raw_mag_x; - int16_t _last_raw_mag_y; - int16_t _last_raw_mag_z; - - hrt_abstime _mag_last_measure{0}; -#endif - - unsigned _call_accel_interval; - - ringbuffer::RingBuffer *_accel_reports; - - struct accel_calibration_s _accel_scale; - unsigned _accel_range_m_s2; - float _accel_range_scale; - unsigned _accel_samplerate; - unsigned _accel_onchip_filter_bandwith; - - - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; - - unsigned _accel_read; - - perf_counter_t _accel_sample_perf; - perf_counter_t _bad_registers; - perf_counter_t _bad_values; - perf_counter_t _accel_duplicates; - - uint8_t _register_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - - Integrator _accel_int; - - enum Rotation _rotation; - - // values used to - float _last_accel[3]; - uint8_t _constant_accel_count; - - // last temperature value - float _last_temperature; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define FXOS8701C_NUM_CHECKED_REGISTERS 5 - static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - void reset(); - - /** - * disable I2C on the chip - */ - void disable_i2c(); - - /** - * check key registers for correct values - */ - void check_registers(void); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measure(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void mag_measure(); - - /** - * Read a register from the FXOS8701C - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the FXOS8701C - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the FXOS8701C - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the FXOS8701C, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Set the FXOS8701C accel measurement range. - * - * @param max_g The measurement range of the accel is in g (9.81m/s^2) - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_range(unsigned max_g); - - /** - * Set the FXOS8701C mag measurement range. - * - * @param max_ga The measurement range of the mag is in Ga - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int mag_set_range(unsigned max_g); - - /** - * Set the FXOS8701C on-chip anti-alias filter bandwith. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); - - /** - * Set the driver lowpass filter bandwidth. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); - - /** - * Set the FXOS8701C internal accel and mag sampling frequency. - * - * @param frequency The internal accel and mag sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int accel_set_samplerate(unsigned frequency); - - /** - * Set the FXOS8701CQ internal mag sampling frequency. - * - * @param frequency The mag reporting frequency is set to not less than - * this value. (sampling is all way the same as accel - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int mag_set_samplerate(unsigned frequency); - - - - /* this class cannot be copied */ - FXOS8701CQ(const FXOS8701CQ &); - FXOS8701CQ operator=(const FXOS8701CQ &); -}; - -/* - list of registers that will be checked in check_registers(). Note - that ADDR_WHO_AM_I must be first in the list. - */ -const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = { - FXOS8701CQ_WHOAMI, - FXOS8701CQ_XYZ_DATA_CFG, - FXOS8701CQ_CTRL_REG1, - FXOS8701CQ_M_CTRL_REG1, - FXOS8701CQ_M_CTRL_REG2, -}; - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -/** - * Helper class implementing the mag driver node. - */ -class FXOS8701CQ_mag : public device::CDev -{ -public: - FXOS8701CQ_mag(FXOS8701CQ *parent); - ~FXOS8701CQ_mag(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class FXOS8701CQ; - - void parent_poll_notify(); -private: - FXOS8701CQ *_parent; - - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; - - void measure(); - - /* this class does not allow copying due to ptr data members */ - FXOS8701CQ_mag(const FXOS8701CQ_mag &); - FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &); -}; -#endif - -FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) : - SPI("FXOS8701CQ", path, bus, device, SPIDEV_MODE0, - 1 * 1000 * 1000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag(new FXOS8701CQ_mag(this)), - _call_mag_interval(0), - _mag_reports(nullptr), - _mag_scale{}, - _mag_range_ga(0.0f), - _mag_range_scale(0.0f), - _mag_samplerate(0), - _mag_read(0), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "fxos8701cq_mag_read")), - _last_raw_mag_x(0), - _last_raw_mag_y(0), - _last_raw_mag_z(0), -#endif - _call_accel_interval(0), - _accel_reports(nullptr), - _accel_scale{}, - _accel_range_m_s2(0.0f), - _accel_range_scale(0.0f), - _accel_samplerate(0), - _accel_onchip_filter_bandwith(0), - _accel_topic(nullptr), - _accel_orb_class_instance(-1), - _accel_class_instance(-1), - _accel_read(0), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "fxos8701cq_acc_read")), - _bad_registers(perf_alloc(PC_COUNT, "fxos8701cq_bad_reg")), - _bad_values(perf_alloc(PC_COUNT, "fxos8701cq_bad_val")), - _accel_duplicates(perf_alloc(PC_COUNT, "fxos8701cq_acc_dupe")), - _register_wait(0), - _accel_filter_x(FXOS8701C_ACCEL_DEFAULT_RATE, FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(FXOS8701C_ACCEL_DEFAULT_RATE, FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(FXOS8701C_ACCEL_DEFAULT_RATE, FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_int(1000000 / FXOS8701C_ACCEL_MAX_OUTPUT_RATE, true), - _rotation(rotation), - _constant_accel_count(0), - _last_temperature(0), - _checked_next(0) -{ - // enable debug() calls - _debug_enabled = false; - - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_FXOS8701C; - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* Prime _mag with parents devid. */ - _mag->_device_id.devid = _device_id.devid; - _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_FXOS8701C; -#endif - - // default scale factors - _accel_scale.x_offset = 0.0f; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0.0f; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0.0f; - _accel_scale.z_scale = 1.0f; - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_scale.x_offset = 0.0f; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0.0f; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0.0f; - _mag_scale.z_scale = 1.0f; -#endif -} - -FXOS8701CQ::~FXOS8701CQ() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; - } - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - - if (_mag_reports != nullptr) { - delete _mag_reports; - } - -#endif - - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - delete _mag; - perf_free(_mag_sample_perf); -#endif - - /* delete the perf counter */ - perf_free(_accel_sample_perf); - perf_free(_bad_registers); - perf_free(_bad_values); - perf_free(_accel_duplicates); -} - -int -FXOS8701CQ::init() -{ - int ret = PX4_ERROR; - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - PX4_ERR("SPI init failed"); - return ret; - } - - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - - if (_accel_reports == nullptr) { - return ret; - } - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - - if (_mag_reports == nullptr) { - return ret; - } - -#endif - - // set software low pass filter for controllers - param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); - float accel_cut = FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - - if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) { - accel_set_driver_lowpass_filter(FXOS8701C_ACCEL_DEFAULT_RATE, accel_cut); - - } else { - PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); - } - - reset(); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* do CDev init for the mag device node */ - ret = _mag->init(); - - if (ret != OK) { - PX4_ERR("MAG init failed"); - return PX4_ERROR; - } - -#endif - - /* fill report structures */ - measure(); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); - - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); - - if (_mag->_mag_topic == nullptr) { - PX4_ERR("ADVERT ERR"); - return PX4_ERROR; - } - -#endif - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_accel_s arp; - _accel_reports->get(&arp); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); - - if (_accel_topic == nullptr) { - PX4_ERR("ADVERT ERR"); - return PX4_ERROR; - } - - return PX4_OK; -} - - -void -FXOS8701CQ::reset() -{ - - /* enable accel set it To Standby */ - - write_checked_reg(FXOS8701CQ_CTRL_REG1, 0); - write_checked_reg(FXOS8701CQ_XYZ_DATA_CFG, 0); - - /* Use hybird mode to read Accel and Mag */ - - write_checked_reg(FXOS8701CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); - - /* Use the hybird auto increment mode to read all the data at the same time */ - - write_checked_reg(FXOS8701CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC); - - accel_set_range(FXOS8701C_ACCEL_DEFAULT_RANGE_G); - accel_set_samplerate(FXOS8701C_ACCEL_DEFAULT_RATE); - accel_set_driver_lowpass_filter((float)FXOS8701C_ACCEL_DEFAULT_RATE, (float)FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - - // we setup the anti-alias on-chip filter as 50Hz. We believe - // this operates in the analog domain, and is critical for - // anti-aliasing. The 2 pole software filter is designed to - // operate in conjunction with this on-chip filter - accel_set_onchip_lowpass_filter_bandwidth(FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - mag_set_range(FXOS8701C_MAG_DEFAULT_RANGE_GA); -#endif - /* enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode */ - - write_checked_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE); - - _accel_read = 0; -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_read = 0; -#endif -} - -int -FXOS8701CQ::probe() -{ - /* verify that the device is attached and functioning */ - uint8_t whoami = read_reg(FXOS8701CQ_WHOAMI); - bool success = (whoami == FXOS8700CQ_WHOAMI_VAL) || (whoami == FXOS8701CQ_WHOAMI_VAL); - - if (success) { - _checked_values[0] = whoami; - return OK; - } - - return -EIO; -} - -ssize_t -FXOS8701CQ::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_accel_s); - sensor_accel_s *arb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_accel_interval > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_accel_reports->get(arb)) { - ret += sizeof(*arb); - arb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - measure(); - - /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) { - ret = sizeof(*arb); - } - - return ret; -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -ssize_t -FXOS8701CQ::mag_read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - mag_report *mrb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_call_mag_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_mag_reports->get(mrb)) { - ret += sizeof(*mrb); - mrb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _mag_reports->flush(); - _mag->measure(); - - /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) { - ret = sizeof(*mrb); - } - - return ret; -} -#endif - -int -FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, FXOS8701C_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / arg; - - /* check against maximum sane rate */ - if (interval < 500) { - return -EINVAL; - } - - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_accel_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - reset(); - return OK; - - case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - - } else { - return -EINVAL; - } - } - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -int -FXOS8701CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - /* 100 Hz is max for mag */ - return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_call_mag_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / arg; - - /* check against maximum sane rate */ - if (interval < 1000) { - return -EINVAL; - } - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_mag_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - reset(); - return OK; - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCGEXTERNAL: - /* Even if this sensor is on the "external" SPI bus - * it is still fixed to the autopilot assembly, - * so always return 0. - */ - return 0; - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} -#endif - -uint8_t -FXOS8701CQ::read_reg(unsigned reg) -{ - uint8_t cmd[3]; - - cmd[0] = DIR_READ(reg); - cmd[1] = ADDR_7(reg); - cmd[2] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[2]; -} - -void -FXOS8701CQ::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[3]; - - cmd[0] = DIR_WRITE(reg); - cmd[1] = ADDR_7(reg); - cmd[2] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -FXOS8701CQ::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -void -FXOS8701CQ::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - -int -FXOS8701CQ::accel_set_range(unsigned max_g) -{ - uint8_t setbits = 0; - float lsb_per_g; - float max_accel_g; - - if (max_g == 0 || max_g > 8) { - max_g = 8; - } - - if (max_g > 4) { // 8g - setbits = XYZ_DATA_CFG_FS_8G; - lsb_per_g = 1024; - max_accel_g = 8; - - } else if (max_g > 2) { // 4g - setbits = XYZ_DATA_CFG_FS_4G; - lsb_per_g = 2048; - max_accel_g = 4; - - } else { // 2g - setbits = XYZ_DATA_CFG_FS_2G; - lsb_per_g = 4096; - max_accel_g = 2; - } - - _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); - _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; - - - modify_reg(FXOS8701CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits); - - return OK; -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -int -FXOS8701CQ::mag_set_range(unsigned max_ga) -{ - _mag_range_ga = 12; - _mag_range_scale = 0.001f; - - return OK; -} -#endif - -int -FXOS8701CQ::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) -{ - return OK; -} - -int -FXOS8701CQ::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) -{ - _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); - - return OK; -} - -int -FXOS8701CQ::accel_set_samplerate(unsigned frequency) -{ - uint8_t setbits = 0; - - /* The selected ODR is reduced by a factor of two when the device is operated in hybrid mode.*/ - - uint8_t active = read_reg(FXOS8701CQ_CTRL_REG1) & CTRL_REG1_ACTIVE; - - if (frequency == 0) { - frequency = FXOS8701C_ACCEL_DEFAULT_RATE; - } - - if (frequency <= 25) { - setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2 - _accel_samplerate = 25; - - } else if (frequency <= 50) { - setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2 - _accel_samplerate = 50; - - } else if (frequency <= 100) { - setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2 - _accel_samplerate = 100; - - } else if (frequency <= 200) { - setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2; - _accel_samplerate = 200; - - } else if (frequency <= 400) { - setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2; - _accel_samplerate = 400; - - } else { - return -EINVAL; - } - - modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0); - modify_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits); - modify_reg(FXOS8701CQ_CTRL_REG1, 0, active); - return OK; -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -int -FXOS8701CQ::mag_set_samplerate(unsigned frequency) -{ - if (frequency == 0) { - frequency = 100; - } - - if (frequency <= 25) { - _mag_samplerate = 25; - - } else if (frequency <= 50) { - _mag_samplerate = 50; - - } else if (frequency <= 100) { - _mag_samplerate = 100; - - } else if (frequency <= 200) { - _mag_samplerate = 200; - - } else if (frequency <= 400) { - _mag_samplerate = 400; - - } else { - return -EINVAL; - } - - return OK; -} -#endif - -void -FXOS8701CQ::start() -{ - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _accel_reports->flush(); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_reports->flush(); -#endif - - /* start polling at the specified rate */ - ScheduleOnInterval(_call_accel_interval - FXOS8701C_TIMER_REDUCTION, 10000); -} - -void -FXOS8701CQ::stop() -{ - ScheduleClear(); - - /* reset internal states */ - memset(_last_accel, 0, sizeof(_last_accel)); - - /* discard unread data in the buffers */ - _accel_reports->flush(); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_reports->flush(); -#endif -} - -void -FXOS8701CQ::Run() -{ - /* make another measurement */ - measure(); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - - if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) { - mag_measure(); - } - -#endif -} - -void -FXOS8701CQ::check_registers(void) -{ - - uint8_t v; - - if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the SPI bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the WHO_AM_I, which - is not writeable - */ - if (_checked_next != 0) { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - } - - _register_wait = 20; - } - - _checked_next = (_checked_next + 1) % FXOS8701C_NUM_CHECKED_REGISTERS; -} - -void -FXOS8701CQ::measure() -{ - /* status register and data as read back from the device */ - -#pragma pack(push, 1) - struct { - uint8_t cmd[2]; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - int16_t mx; - int16_t my; - int16_t mz; - } raw_accel_mag_report; -#pragma pack(pop) - - sensor_accel_s accel_report; - - /* start the performance counter */ - perf_begin(_accel_sample_perf); - - check_registers(); - - if (_register_wait != 0) { - // we are waiting for some good transfers before using - // the sensor again. - _register_wait--; - perf_end(_accel_sample_perf); - return; - } - - /* fetch data from the sensor */ - memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report)); - raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8701CQ_DR_STATUS); - raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8701CQ_DR_STATUS); - transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); - - if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) { - perf_end(_accel_sample_perf); - perf_count(_accel_duplicates); - return; - } - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - accel_report.timestamp = hrt_absolute_time(); - - /* - * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. - * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor - * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the - * temperature sensor is uncalibrated and its output for a given temperature will vary from - * one device to the next - */ - - _last_temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f; - - accel_report.temperature = _last_temperature; - - // report the error count as the sum of the number of bad - // register reads and bad values. This allows the higher level - // code to decide if it should use this sensor based on - // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - - accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x); - accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y); - accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* Save off the Mag readings todo: revist integrating theses */ - - _last_raw_mag_x = swap16(raw_accel_mag_report.mx); - _last_raw_mag_y = swap16(raw_accel_mag_report.my); - _last_raw_mag_z = swap16(raw_accel_mag_report.mz); -#endif - - float xraw_f = accel_report.x_raw; - float yraw_f = accel_report.y_raw; - float zraw_f = accel_report.z_raw; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - /* - we have logs where the accelerometers get stuck at a fixed - large value. We want to detect this and mark the sensor as - being faulty - */ - if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - fabsf(_last_accel[1] - y_in_new) < 0.001f && - fabsf(_last_accel[2] - z_in_new) < 0.001f && - fabsf(x_in_new) > 20 && - fabsf(y_in_new) > 20 && - fabsf(z_in_new) > 20) { - _constant_accel_count += 1; - - } else { - _constant_accel_count = 0; - } - - if (_constant_accel_count > 100) { - // we've had 100 constant accel readings with large - // values. The sensor is almost certainly dead. We - // will raise the error_count so that the top level - // flight code will know to avoid this sensor, but - // we'll still give the data so that it can be logged - // and viewed - perf_count(_bad_values); - _constant_accel_count = 0; - } - - _last_accel[0] = x_in_new; - _last_accel[1] = y_in_new; - _last_accel[2] = z_in_new; - - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); - - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; - - bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); - - accel_report.scaling = _accel_range_scale; - - /* return device ID */ - accel_report.device_id = _device_id.devid; - - _accel_reports->force(&accel_report); - - /* notify anyone waiting for data */ - if (accel_notify) { - poll_notify(POLLIN); - - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } - } - - _accel_read++; - - /* stop the perf counter */ - perf_end(_accel_sample_perf); -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -void -FXOS8701CQ::mag_measure() -{ - /* status register and data as read back from the device */ - - mag_report mag_report {}; - - /* start the performance counter */ - perf_begin(_mag_sample_perf); - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - mag_report.timestamp = hrt_absolute_time(); - - _mag_last_measure = mag_report.timestamp; - - mag_report.is_external = external(); - - mag_report.x_raw = _last_raw_mag_x; - mag_report.y_raw = _last_raw_mag_y; - mag_report.z_raw = _last_raw_mag_z; - - float xraw_f = mag_report.x_raw; - float yraw_f = mag_report.y_raw; - float zraw_f = mag_report.z_raw; - - /* apply user specified rotation */ - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.scaling = _mag_range_scale; - mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - - mag_report.temperature = _last_temperature; - mag_report.device_id = _mag->_device_id.devid; - - _mag_reports->force(&mag_report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - if (!(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); - } - - _mag_read++; - - /* stop the perf counter */ - perf_end(_mag_sample_perf); -} -#endif - -void -FXOS8701CQ::print_info() -{ - printf("accel reads: %u\n", _accel_read); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - printf("mag reads: %u\n", _mag_read); -#endif - perf_print_counter(_accel_sample_perf); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - perf_print_counter(_mag_sample_perf); -#endif - perf_print_counter(_bad_registers); - perf_print_counter(_bad_values); - perf_print_counter(_accel_duplicates); - _accel_reports->print_info("accel reports"); -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _mag_reports->print_info("mag reports"); -#endif - ::printf("checked_next: %u\n", _checked_next); - - for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) { - uint8_t v = read_reg(_checked_registers[i]); - - if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_values[i]); - } - } - - ::printf("temperature: %.2f\n", (double)_last_temperature); -} - -void -FXOS8701CQ::print_registers() -{ - const struct { - uint8_t reg; - const char *name; - } regmap[] = { - DEF_REG(FXOS8701CQ_DR_STATUS), - DEF_REG(FXOS8701CQ_OUT_X_MSB), - DEF_REG(FXOS8701CQ_XYZ_DATA_CFG), - DEF_REG(FXOS8701CQ_WHOAMI), - DEF_REG(FXOS8701CQ_CTRL_REG1), - DEF_REG(FXOS8701CQ_CTRL_REG2), - DEF_REG(FXOS8701CQ_M_DR_STATUS), - DEF_REG(FXOS8701CQ_M_OUT_X_MSB), - DEF_REG(FXOS8701CQ_M_CTRL_REG1), - DEF_REG(FXOS8701CQ_M_CTRL_REG2), - DEF_REG(FXOS8701CQ_M_CTRL_REG3), - }; - - for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) { - printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); - } -} - -void -FXOS8701CQ::test_error() -{ - // trigger an error - write_reg(FXOS8701CQ_CTRL_REG1, 0); -} - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -FXOS8701CQ_mag::FXOS8701CQ_mag(FXOS8701CQ *parent) : - CDev("FXOS8701C_mag", FXOS8701C_DEVICE_PATH_MAG), - _parent(parent), - _mag_topic(nullptr), - _mag_orb_class_instance(-1), - _mag_class_instance(-1) -{ -} - -FXOS8701CQ_mag::~FXOS8701CQ_mag() -{ - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } -} - -int -FXOS8701CQ_mag::init() -{ - int ret; - - ret = CDev::init(); - - if (ret == OK) { - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - } - - return ret; -} - -void -FXOS8701CQ_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -ssize_t -FXOS8701CQ_mag::read(struct file *filp, char *buffer, size_t buflen) -{ - return _parent->mag_read(filp, buffer, buflen); -} - -int -FXOS8701CQ_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case DEVIOCGDEVICEID: - return (int)CDev::ioctl(filp, cmd, arg); - break; - - default: - return _parent->mag_ioctl(filp, cmd, arg); - } -} - -void -FXOS8701CQ_mag::measure() -{ - _parent->mag_measure(); -} - -#endif - -/** - * Local functions in support of the shell command. - */ -namespace fxos8701cq -{ - -FXOS8701CQ *g_dev; - -void start(bool external_bus, enum Rotation rotation); -void test(); -void reset(); -void info(); -void stop(); -void regdump(); -void usage(); -void test_error(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -void -start(bool external_bus, enum Rotation rotation) -{ - int fd; - - if (g_dev != nullptr) { - PX4_INFO("already started"); - exit(0); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) - g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); -#else - PX4_ERR("External SPI not available"); - exit(0); -#endif - - } else { - g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_ACCEL_MAG, rotation); - } - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating FXOS8701C obj"); - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - int fd_mag; - fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY); - - /* don't fail if open cannot be opened */ - if (0 <= fd_mag) { - if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - } - - close(fd_mag); -#endif - - close(fd); - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - int rv = 1; - int fd_accel = -1; - sensor_accel_s accel_report; - ssize_t sz; -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - int fd_mag = -1; - int ret; - struct mag_report m_report; -#endif - - /* get the driver */ - fd_accel = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY); - - if (fd_accel < 0) { - PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_ACCEL); - goto exit_none; - } - - /* do a simple demand read */ - sz = read(fd_accel, &accel_report, sizeof(sensor_accel_s)); - - if (sz != sizeof(sensor_accel_s)) { - PX4_ERR("immediate read failed"); - goto exit_with_accel; - } - - - PX4_INFO("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); - PX4_INFO("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); - PX4_INFO("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); - PX4_INFO("accel x: \t%d\traw", (int)accel_report.x_raw); - PX4_INFO("accel y: \t%d\traw", (int)accel_report.y_raw); - PX4_INFO("accel z: \t%d\traw", (int)accel_report.z_raw); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - /* get the driver */ - fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY); - - if (fd_mag < 0) { - PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_MAG); - goto exit_with_accel; - } - - /* check if mag is onboard or external */ - if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) { - PX4_ERR("failed to get if mag is onboard or external"); - goto exit_with_mag_accel; - } - - PX4_INFO("mag device active: %s", ret ? "external" : "onboard"); - - /* do a simple demand read */ - sz = read(fd_mag, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) { - PX4_ERR("immediate read failed"); - goto exit_with_mag_accel; - } - - PX4_INFO("mag x: \t% 9.5f\tga", (double)m_report.x); - PX4_INFO("mag y: \t% 9.5f\tga", (double)m_report.y); - PX4_INFO("mag z: \t% 9.5f\tga", (double)m_report.z); - PX4_INFO("mag x: \t%d\traw", (int)m_report.x_raw); - PX4_INFO("mag y: \t%d\traw", (int)m_report.y_raw); - PX4_INFO("mag z: \t%d\traw", (int)m_report.z_raw); -#endif - - /* reset to default polling */ - if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("reset to default polling"); - - } else { - rv = 0; - } - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -exit_with_mag_accel: - close(fd_mag); -#endif - -exit_with_accel: - - close(fd_accel); - - reset(); - - if (rv == 0) { - PX4_INFO("PASS"); - } - -exit_none: - exit(rv); -} - -/** - * Reset the driver. - */ -void -reset() -{ - int fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY); - int rv = 1; - - if (fd < 0) { - PX4_ERR("Open failed\n"); - exit(1); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - exit(1); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("accel pollrate reset failed"); - exit(1); - } - - close(fd); - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - fd = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY); - - if (fd < 0) { - PX4_ERR("mag could not be opened, external mag might be used"); - - } else { - /* no need to reset the mag as well, the reset() is the same */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("mag pollrate reset failed"); - - } else { - rv = 0; - } - } - - close(fd); -#endif - - exit(rv); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -void -stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - delete g_dev; - g_dev = nullptr; - - exit(0); -} - -/** - * dump registers from device - */ -void -regdump() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - printf("regdump @ %p\n", g_dev); - g_dev->print_registers(); - - exit(0); -} - -/** - * trigger an error - */ -void -test_error() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running\n"); - exit(1); - } - - g_dev->test_error(); - - exit(0); -} - -void -usage() -{ - PX4_INFO("missing command: try 'start', 'info', 'stop', 'test', 'reset', 'testerror' or 'regdump'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); -} - -} // namespace - -int -fxos8701cq_main(int argc, char *argv[]) -{ - bool external_bus = false; - int ch; - enum Rotation rotation = ROTATION_NONE; - - int myoptind = 1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - external_bus = true; - break; - - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - fxos8701cq::usage(); - exit(0); - } - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - - */ - if (!strcmp(verb, "start")) { - fxos8701cq::start(external_bus, rotation); - } - - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - fxos8701cq::test(); - } - - if (!strcmp(verb, "stop")) { - fxos8701cq::stop(); - } - - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - fxos8701cq::reset(); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - fxos8701cq::info(); - } - - /* - * dump device registers - */ - if (!strcmp(verb, "regdump")) { - fxos8701cq::regdump(); - } - - /* - * trigger an error - */ - if (!strcmp(verb, "testerror")) { - fxos8701cq::test_error(); - } - - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset', 'info', 'testerror' or 'regdump'"); -} diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp new file mode 100644 index 000000000000..147bac5944da --- /dev/null +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * Copyright (c) 2017-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 fxos8701cq.cpp + * Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and + * magnetometer connected via SPI. + */ + +#include "FXOS8701CQ.hpp" + +#include + +/** + * Local functions in support of the shell command. + */ +namespace fxos8701cq +{ + +FXOS8701CQ *g_dev{nullptr}; + +int start(bool external_bus, enum Rotation rotation); +int info(); +int stop(); +int regdump(); +int usage(); +int test_error(); + +/** + * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. + */ +int +start(bool external_bus, enum Rotation rotation) +{ + if (g_dev != nullptr) { + PX4_INFO("already started"); + return 0; + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) + g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); +#else + PX4_ERR("External SPI not available"); + return 0; +#endif + + } else { + g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); + } + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating FXOS8701C obj"); + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + return PX4_OK; +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + PX4_ERR("driver start failed"); + return PX4_ERROR; +} + +/** + * Print a little info about the driver. + */ +int +info() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running\n"); + return 1; + } + + g_dev->print_info(); + + return 0; +} + +int +stop() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running\n"); + return 1; + } + + delete g_dev; + g_dev = nullptr; + + return 0; +} + +/** + * dump registers from device + */ +int +regdump() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running\n"); + return 1; + } + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + return 0; +} + +/** + * trigger an error + */ +int +test_error() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running\n"); + return 1; + } + + g_dev->test_error(); + + return 0; +} + +int +usage() +{ + PX4_INFO("missing command: try 'start', 'info', 'stop', 'testerror' or 'regdump'"); + PX4_INFO("options:"); + PX4_INFO(" -X (external bus)"); + PX4_INFO(" -R rotation"); + + return 0; +} + +} // namespace + +extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); } + +int fxos8701cq_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + + int myoptind = 1; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + fxos8701cq::usage(); + exit(0); + } + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return fxos8701cq::start(external_bus, rotation); + + } else if (!strcmp(verb, "stop")) { + return fxos8701cq::stop(); + + } else if (!strcmp(verb, "info")) { + return fxos8701cq::info(); + + } else if (!strcmp(verb, "regdump")) { + return fxos8701cq::regdump(); + + } else if (!strcmp(verb, "testerror")) { + return fxos8701cq::test_error(); + } + + PX4_ERR("unrecognized command, try 'start', 'stop', 'info', 'testerror' or 'regdump'"); + return PX4_ERROR; +} diff --git a/src/drivers/imu/icm20948/CMakeLists.txt b/src/drivers/imu/icm20948/CMakeLists.txt index e32a67fa37c4..0d7dcbbae8c7 100644 --- a/src/drivers/imu/icm20948/CMakeLists.txt +++ b/src/drivers/imu/icm20948/CMakeLists.txt @@ -33,16 +33,13 @@ px4_add_module( MODULE drivers__icm20948 MAIN icm20948 - STACK_MAIN 1500 COMPILE_FLAGS SRCS icm20948.cpp icm20948_i2c.cpp icm20948_spi.cpp - main.cpp - accel.cpp - gyro.cpp - mag.cpp + icm20948_main.cpp + ICM20948_mag.cpp mag_i2c.cpp DEPENDS ) diff --git a/src/drivers/imu/icm20948/mag.cpp b/src/drivers/imu/icm20948/ICM20948_mag.cpp similarity index 59% rename from src/drivers/imu/icm20948/mag.cpp rename to src/drivers/imu/icm20948/ICM20948_mag.cpp index 002c04639438..ccdbecc26121 100644 --- a/src/drivers/imu/icm20948/mag.cpp +++ b/src/drivers/imu/icm20948/ICM20948_mag.cpp @@ -40,56 +40,34 @@ * */ -#include "mag.h" -#include "icm20948.h" +#include +#include +#include +#include +#include +#include "ICM20948_mag.h" +#include "icm20948.h" // If interface is non-null, then it will used for interacting with the device. // Otherwise, it will passthrough the parent ICM20948 -ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path) : - CDev("ICM20948_mag", path), +ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation) : _interface(interface), + _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), + rotation), _parent(parent), - _mag_topic(nullptr), - _mag_orb_class_instance(-1), - _mag_class_instance(-1), - _mag_reading_data(false), - _mag_reports(nullptr), - _mag_scale{}, - _mag_range_scale(), - _mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_reads")), - _mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors")), - _mag_overruns(perf_alloc(PC_COUNT, "mpu9250_mag_overruns")), - _mag_overflows(perf_alloc(PC_COUNT, "mpu9250_mag_overflows")), - _mag_duplicates(perf_alloc(PC_COUNT, "mpu9250_mag_duplicates")), - _mag_asa_x(1.0), - _mag_asa_y(1.0), - _mag_asa_z(1.0), - _last_mag_data{} + _mag_reads(perf_alloc(PC_COUNT, "icm20948: mag_reads")), + _mag_errors(perf_alloc(PC_COUNT, "icm20948: mag_errors")), + _mag_overruns(perf_alloc(PC_COUNT, "icm20948: mag_overruns")), + _mag_overflows(perf_alloc(PC_COUNT, "icm20948: mag_overflows")), + _mag_duplicates(perf_alloc(PC_COUNT, "icm20948: mag_duplicates")) { - // default mag scale factors - _mag_scale.x_offset = 0; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; - _mag_scale.z_scale = 1.0f; - - _mag_range_scale = MPU9250_MAG_RANGE_GA; + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); + _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); } ICM20948_mag::~ICM20948_mag() { - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } - - if (_mag_reports != nullptr) { - delete _mag_reports; - } - - orb_unadvertise(_mag_topic); - perf_free(_mag_reads); perf_free(_mag_errors); perf_free(_mag_overruns); @@ -97,43 +75,6 @@ ICM20948_mag::~ICM20948_mag() perf_free(_mag_duplicates); } -int -ICM20948_mag::init() -{ - int ret = CDev::init(); - - /* if cdev init failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("ICM20948 mag init failed"); - - return ret; - } - - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - - if (_mag_reports == nullptr) { - return -ENOMEM; - } - - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); - - - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); - - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag_orb_class_instance, (_parent->is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); -// &_mag_orb_class_instance, ORB_PRIO_LOW); - - if (_mag_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return -ENOMEM; - } - - return OK; -} - bool ICM20948_mag::check_duplicate(uint8_t *mag_data) { if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { @@ -149,27 +90,25 @@ bool ICM20948_mag::check_duplicate(uint8_t *mag_data) void ICM20948_mag::measure() { - uint8_t ret; union raw_data_t { struct ak8963_regs ak8963_data; struct ak09916_regs ak09916_data; } raw_data; - ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); if (ret == OK) { raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; - _measure(raw_data.ak8963_data); + _measure(timestamp_sample, raw_data.ak8963_data); } } void -ICM20948_mag::_measure(struct ak8963_regs data) +ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data) { - bool mag_notify = true; - if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { perf_count(_mag_duplicates); return; @@ -188,119 +127,14 @@ ICM20948_mag::_measure(struct ak8963_regs data) perf_count(_mag_overflows); } - mag_report mrb; - mrb.timestamp = hrt_absolute_time(); -// mrb.is_external = false; - - // need a better check here. Using _parent->is_external() for mpu9250 also sets the - // internal magnetometers connected to the "external" spi bus as external, at least - // on Pixhawk 2.1. For now assuming the ICM20948 is only used on Here GPS, hence external. - if (_parent->_whoami == ICM_WHOAMI_20948) { - mrb.is_external = _parent->is_external(); - - } else { - mrb.is_external = false; - } - - /* - * Align axes - note the accel & gryo are also re-aligned so this - * doesn't look obvious with the datasheet - */ - float xraw_f, yraw_f, zraw_f; - - if (_parent->_whoami == ICM_WHOAMI_20948) { - /* - * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. - */ - mrb.x_raw = data.y; - mrb.y_raw = data.x; - mrb.z_raw = -data.z; - - xraw_f = data.y; - yraw_f = data.x; - zraw_f = -data.z; - - } else { - mrb.x_raw = data.x; - mrb.y_raw = -data.y; - mrb.z_raw = -data.z; - - xraw_f = data.x; - yraw_f = -data.y; - zraw_f = -data.z; - } - - /* apply user specified rotation */ - rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f); - - if (_parent->_whoami == ICM_WHOAMI_20948) { - rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948 - } - - mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale; - mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale; - mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale; - mrb.scaling = _mag_range_scale; - mrb.temperature = _parent->_last_temperature; - mrb.device_id = _parent->_mag->_device_id.devid; - - mrb.error_count = perf_event_count(_mag_errors); - - _mag_reports->force(&mrb); - - /* notify anyone waiting for data */ - if (mag_notify) { - poll_notify(POLLIN); - } - - if (mag_notify && !(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb); - } -} + _px4_mag.set_external(_parent->is_external()); + _px4_mag.set_temperature(_parent->_last_temperature); + _px4_mag.set_error_count(perf_event_count(_mag_errors)); -int -ICM20948_mag::ioctl(struct file *filp, int cmd, unsigned long arg) -{ /* - * Repeated in ICM20948_accel::ioctl - * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 + * Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. */ - - switch (cmd) { - - case SENSORIOCRESET: - return ak8963_reset(); - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: - return _parent->_set_pollrate(arg); - } - } - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - default: - return (int)CDev::ioctl(filp, cmd, arg); - } + _px4_mag.update(timestamp_sample, data.y, data.x, -data.z); } void @@ -341,7 +175,7 @@ ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) uint8_t ICM20948_mag::read_reg(unsigned int reg) { - uint8_t buf; + uint8_t buf{}; if (_interface == nullptr) { passthrough_read(reg, &buf, 0x01); @@ -428,9 +262,7 @@ ICM20948_mag::ak8963_read_adjustments(void) } } - _mag_asa_x = ak8963_ASA[0]; - _mag_asa_y = ak8963_ASA[1]; - _mag_asa_z = ak8963_ASA[2]; + _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]); return true; } diff --git a/src/drivers/imu/icm20948/mag.h b/src/drivers/imu/icm20948/ICM20948_mag.h similarity index 87% rename from src/drivers/imu/icm20948/mag.h rename to src/drivers/imu/icm20948/ICM20948_mag.h index b84a407ecbaa..bfb580ed6dce 100644 --- a/src/drivers/imu/icm20948/mag.h +++ b/src/drivers/imu/icm20948/ICM20948_mag.h @@ -33,16 +33,12 @@ #pragma once -#include -#include -#include -#include -#include -#include +#include +#include +#include /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ - -#define MPU9250_MAG_RANGE_GA 1.5e-3f; +static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; /* we are using the continuous fixed sampling rate of 100Hz */ @@ -132,15 +128,12 @@ typedef device::Device *(*ICM20948_mag_constructor)(int, bool); /** * Helper class implementing the magnetometer driver node. */ -class ICM20948_mag : public device::CDev +class ICM20948_mag { public: - ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path); + ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation); ~ICM20948_mag(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - virtual int init(); - void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL); void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size); void passthrough_write(uint8_t reg, uint8_t val); @@ -152,8 +145,10 @@ class ICM20948_mag : public device::CDev bool ak8963_check_id(uint8_t &id); bool ak8963_read_adjustments(void); + void print_status() { _px4_mag.print_status(); } + protected: - Device *_interface; + device::Device *_interface; friend class ICM20948; @@ -161,7 +156,7 @@ class ICM20948_mag : public device::CDev void measure(); /* Update the state with prefetched data (internally called by the regular measure() )*/ - void _measure(struct ak8963_regs data); + void _measure(hrt_abstime timestamp, struct ak8963_regs data); uint8_t read_reg(unsigned reg); void write_reg(unsigned reg, uint8_t value); @@ -169,27 +164,23 @@ class ICM20948_mag : public device::CDev bool is_passthrough() { return _interface == nullptr; } private: + + PX4Magnetometer _px4_mag; + ICM20948 *_parent; - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; - bool _mag_reading_data; - ringbuffer::RingBuffer *_mag_reports; - struct mag_calibration_s _mag_scale; - float _mag_range_scale; + + bool _mag_reading_data{false}; + perf_counter_t _mag_reads; perf_counter_t _mag_errors; perf_counter_t _mag_overruns; perf_counter_t _mag_overflows; perf_counter_t _mag_duplicates; - float _mag_asa_x; - float _mag_asa_y; - float _mag_asa_z; bool check_duplicate(uint8_t *mag_data); // keep last mag reading for duplicate detection - uint8_t _last_mag_data[6]; + uint8_t _last_mag_data[6] {}; /* do not allow to copy this class due to pointer data members */ ICM20948_mag(const ICM20948_mag &); diff --git a/src/drivers/imu/icm20948/accel.cpp b/src/drivers/imu/icm20948/accel.cpp deleted file mode 100644 index fba32ba74635..000000000000 --- a/src/drivers/imu/icm20948/accel.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 gyro.cpp - * - * Driver for the Invensense mpu9250 connected via SPI. - * - * @author Andrew Tridgell - * - * based on the mpu6000 driver - */ - -#include - -#include "accel.h" -#include "icm20948.h" - -ICM20948_accel::ICM20948_accel(ICM20948 *parent, const char *path) : - CDev("ICM20948_accel", path), - _parent(parent) -{ -} - -ICM20948_accel::~ICM20948_accel() -{ - if (_accel_class_instance != -1) { - unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); - } -} - -int -ICM20948_accel::init() -{ - // do base class init - int ret = CDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("accel init failed"); - return ret; - } - - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - - return ret; -} - -void -ICM20948_accel::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -int -ICM20948_accel::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* - * Repeated in ICM20948_mag::ioctl - * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 - */ - - switch (cmd) { - case SENSORIOCRESET: { - return _parent->reset(); - } - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: - return _parent->_set_pollrate(arg); - } - } - - case ACCELIOCSSCALE: { - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale)); - return OK; - } - - default: - return CDev::ioctl(filp, cmd, arg); - } -} diff --git a/src/drivers/imu/icm20948/gyro.cpp b/src/drivers/imu/icm20948/gyro.cpp deleted file mode 100644 index 3523499fb05a..000000000000 --- a/src/drivers/imu/icm20948/gyro.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 gyro.cpp - * - * Driver for the Invensense icm20948 connected via SPI. - * - * - * based on the mpu9250 driver - */ - -#include "gyro.h" -#include "icm20948.h" - -ICM20948_gyro::ICM20948_gyro(ICM20948 *parent, const char *path) : - CDev("ICM20948_gyro", path), - _parent(parent) -{ -} - -ICM20948_gyro::~ICM20948_gyro() -{ - if (_gyro_class_instance != -1) { - unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); - } -} - -int -ICM20948_gyro::init() -{ - // do base class init - int ret = CDev::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("gyro init failed"); - return ret; - } - - _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - - return ret; -} - -void -ICM20948_gyro::parent_poll_notify() -{ - poll_notify(POLLIN); -} - -int -ICM20948_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - /* these are shared with the accel side */ - case SENSORIOCSPOLLRATE: - case SENSORIOCRESET: - return _parent->_accel->ioctl(filp, cmd, arg); - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_parent->_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_parent->_gyro_scale)); - return OK; - - default: - return CDev::ioctl(filp, cmd, arg); - } -} diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index 6027175f4e6b..e499fc7756e1 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,9 +36,21 @@ * * Driver for the Invensense ICM20948 connected via I2C or SPI. * + * * based on the mpu9250 driver */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ICM20948_mag.h" #include "icm20948.h" /* @@ -55,149 +67,54 @@ list of registers that will be checked in check_registers(). Note that MPUREG_PRODUCT_ID must be first in the list. */ -const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { - ICMREG_20948_USER_CTRL, - ICMREG_20948_PWR_MGMT_1, - ICMREG_20948_PWR_MGMT_2, - ICMREG_20948_INT_PIN_CFG, - ICMREG_20948_INT_ENABLE, - ICMREG_20948_INT_ENABLE_1, - ICMREG_20948_INT_ENABLE_2, - ICMREG_20948_INT_ENABLE_3, - ICMREG_20948_GYRO_SMPLRT_DIV, - ICMREG_20948_GYRO_CONFIG_1, - ICMREG_20948_GYRO_CONFIG_2, - ICMREG_20948_ACCEL_SMPLRT_DIV_1, - ICMREG_20948_ACCEL_SMPLRT_DIV_2, - ICMREG_20948_ACCEL_CONFIG, - ICMREG_20948_ACCEL_CONFIG_2 -}; - - -ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, - const char *path_gyro, const char *path_mag, - enum Rotation rotation, +const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { ICMREG_20948_USER_CTRL, + ICMREG_20948_PWR_MGMT_1, + ICMREG_20948_PWR_MGMT_2, + ICMREG_20948_INT_PIN_CFG, + ICMREG_20948_INT_ENABLE, + ICMREG_20948_INT_ENABLE_1, + ICMREG_20948_INT_ENABLE_2, + ICMREG_20948_INT_ENABLE_3, + ICMREG_20948_GYRO_SMPLRT_DIV, + ICMREG_20948_GYRO_CONFIG_1, + ICMREG_20948_GYRO_CONFIG_2, + ICMREG_20948_ACCEL_SMPLRT_DIV_1, + ICMREG_20948_ACCEL_SMPLRT_DIV_2, + ICMREG_20948_ACCEL_CONFIG, + ICMREG_20948_ACCEL_CONFIG_2 + }; + +ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only) : ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), - _whoami(0), - _accel(magnetometer_only ? nullptr : new ICM20948_accel(this, path_accel)), - _gyro(magnetometer_only ? nullptr : new ICM20948_gyro(this, path_gyro)), - _mag(new ICM20948_mag(this, mag_interface, path_mag)), + _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), + _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), + _mag(this, mag_interface, rotation), _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write _magnetometer_only(magnetometer_only), - _call_interval(0), - _accel_reports(nullptr), - _accel_scale{}, - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(nullptr), - _gyro_reports(nullptr), - _gyro_scale{}, - _gyro_range_scale(0.0f), - _gyro_range_rad_s(0.0f), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), _dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), _dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _sample_rate(1000), - _accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")), - _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")), - _bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_trans")), - _bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_reg")), - _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")), - _reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset")), - _duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")), - _register_wait(0), - _reset_wait(0), - _accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), - _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), - _rotation(rotation), - _checked_registers(nullptr), - _checked_next(0), - _num_checked_registers(0), - _last_temperature(0), - _last_accel_data{}, - _got_duplicate(false) + _accel_reads(perf_alloc(PC_COUNT, "icm20948: acc_read")), + _gyro_reads(perf_alloc(PC_COUNT, "icm20948: gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "icm20948: read")), + _bad_transfers(perf_alloc(PC_COUNT, "icm20948: bad_trans")), + _bad_registers(perf_alloc(PC_COUNT, "icm20948: bad_reg")), + _good_transfers(perf_alloc(PC_COUNT, "icm20948: good_trans")), + _reset_retries(perf_alloc(PC_COUNT, "icm20948: reset")), + _duplicates(perf_alloc(PC_COUNT, "icm20948: dupe")) { - if (_accel != nullptr) { - /* Set device parameters and make sure parameters of the bus device are adopted */ - _accel->_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; - _accel->_device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type(); - _accel->_device_id.devid_s.bus = _interface->get_device_bus(); - _accel->_device_id.devid_s.address = _interface->get_device_address(); - } - - if (_gyro != nullptr) { - /* Prime _gyro with common devid. */ - /* Set device parameters and make sure parameters of the bus device are adopted */ - _gyro->_device_id.devid = 0; - _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; - _gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); - _gyro->_device_id.devid_s.bus = _interface->get_device_bus(); - _gyro->_device_id.devid_s.address = _interface->get_device_address(); - } - - /* Prime _mag with common devid. */ - _mag->_device_id.devid = 0; - _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; - _mag->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); - _mag->_device_id.devid_s.bus = _interface->get_device_bus(); - _mag->_device_id.devid_s.address = _interface->get_device_address(); - - // default accel scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - // default gyro scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; + _px4_accel.set_device_type(DRV_DEVTYPE_ICM20948); + _px4_gyro.set_device_type(DRV_DEVTYPE_ICM20948); } ICM20948::~ICM20948() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); - _call_interval = 0; - - if (!_magnetometer_only) { - orb_unadvertise(_accel_topic); - orb_unadvertise(_gyro->_gyro_topic); - } - - /* delete the accel subdriver */ - delete _accel; - - /* delete the gyro subdriver */ - delete _gyro; - /* delete the magnetometer subdriver */ - delete _mag; - - /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; - } - - if (_gyro_reports != nullptr) { - delete _gyro_reports; - } - - /* delete the perf counter */ + // delete the perf counter perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); @@ -211,8 +128,6 @@ ICM20948::~ICM20948() int ICM20948::init() { - irqstate_t state; - /* * If the MPU is using I2C we should reduce the sample rate to 200Hz and * make the integration autoreset faster so that we integrate just one @@ -222,8 +137,6 @@ ICM20948::init() if (is_i2c && !_magnetometer_only) { _sample_rate = 200; - _accel_int.set_autoreset_interval(1000000 / 1000); - _gyro_int.set_autoreset_interval(1000000 / 1000); } int ret = probe(); @@ -233,157 +146,38 @@ ICM20948::init() return ret; } - state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 100000; - px4_leave_critical_section(state); if (reset_mpu() != OK) { PX4_ERR("Exiting! Device failed to take initialization"); return ret; } - if (!_magnetometer_only) { - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - ret = -ENOMEM; - - if (_accel_reports == nullptr) { - return ret; - } - - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - - if (_gyro_reports == nullptr) { - return ret; - } - - /* Initialize offsets and scales */ - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; - - // set software low pass filter for controllers - param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); - float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - - if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) { - PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); - - _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); - - } - - param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); - float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; - - if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) { - PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); - - _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); - - } - - /* do CDev init for the accel device node */ - ret = _accel->init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_DEBUG("accel init failed"); - return ret; - } - - /* do CDev init for the gyro device node */ - ret = _gyro->init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_DEBUG("gyro init failed"); - return ret; - } - } - /* Magnetometer setup */ #ifdef USE_I2C - up_udelay(100); + px4_usleep(100); - if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) { + if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) { PX4_ERR("failed to setup ak8963 interface"); } #endif /* USE_I2C */ - /* do CDev init for the mag device node */ - ret = _mag->init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_DEBUG("mag init failed"); - return ret; - } - - ret = _mag->ak8963_reset(); + ret = _mag.ak8963_reset(); if (ret != OK) { PX4_DEBUG("mag reset failed"); return ret; } - measure(); - - if (!_magnetometer_only) { - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_accel_s arp; - _accel_reports->get(&arp); - - /* measurement will have generated a report, publish */ - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); - - if (_accel_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return ret; - } - - /* advertise sensor topic, measure manually to initialize valid report */ - sensor_gyro_s grp; - _gyro_reports->get(&grp); - - _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); - - if (_gyro->_gyro_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return ret; - } - } + start(); return ret; } -uint8_t ICM20948::get_whoami() -{ - return _whoami; -} - int ICM20948::reset() { - irqstate_t state; - /* When the mpu9250 starts from 0V the internal power on circuit * per the data sheet will require: * @@ -394,21 +188,15 @@ int ICM20948::reset() px4_usleep(110000); // Hold off sampling until done (100 MS will be shortened) - state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 100000; - px4_leave_critical_section(state); - - int ret; - ret = reset_mpu(); + int ret = reset_mpu(); if (ret == OK && (_whoami == ICM_WHOAMI_20948)) { - ret = _mag->ak8963_reset(); + ret = _mag.ak8963_reset(); } - state = px4_enter_critical_section(); _reset_wait = hrt_absolute_time() + 10; - px4_leave_critical_section(state); return ret; } @@ -450,17 +238,15 @@ int ICM20948::reset_mpu() // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s // scaling factor: // 1/(2^15)*(2000/180)*PI - _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); - _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; + _px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready - write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1), - BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C - bool bypass = !_mag->is_passthrough(); + bool bypass = !_mag.is_passthrough(); #else bool bypass = false; #endif @@ -474,11 +260,9 @@ int ICM20948::reset_mpu() * so bypass is true if the mag has an i2c non null interfaces. */ - write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG), - BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); + write_checked_reg(ICMREG_20948_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); - write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2), - MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32)); + write_checked_reg(ICMREG_20948_ACCEL_CONFIG_2, ICM_BITS_DEC3_CFG_32); retries = 3; bool all_ok = false; @@ -573,53 +357,6 @@ ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz) } } -/* - * Set poll rate - */ -int -ICM20948::_set_pollrate(unsigned long rate) -{ - if (rate == 0) { - return -EINVAL; - - } else { - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / rate; - - /* check against maximum sane rate */ - if (interval < 1000) { - return -EINVAL; - } - - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f / interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - - - float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); - _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); - - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } -} - /* set the DLPF filter frequency. This affects both accel and gyro. */ @@ -635,39 +372,39 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz) choose next highest filter frequency available for gyroscope */ if (frequency_hz == 0) { - _dlpf_freq_icm_gyro = 0; + //_dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } else if (frequency_hz <= 5) { - _dlpf_freq_icm_gyro = 5; + //_dlpf_freq_icm_gyro = 5; filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { - _dlpf_freq_icm_gyro = 11; + //_dlpf_freq_icm_gyro = 11; filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { - _dlpf_freq_icm_gyro = 23; + //_dlpf_freq_icm_gyro = 23; filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; } else if (frequency_hz <= 51) { - _dlpf_freq_icm_gyro = 51; + //_dlpf_freq_icm_gyro = 51; filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; } else if (frequency_hz <= 119) { - _dlpf_freq_icm_gyro = 119; + //_dlpf_freq_icm_gyro = 119; filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; } else if (frequency_hz <= 151) { - _dlpf_freq_icm_gyro = 151; + //_dlpf_freq_icm_gyro = 151; filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; } else if (frequency_hz <= 197) { - _dlpf_freq_icm_gyro = 197; + //_dlpf_freq_icm_gyro = 197; filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; } else { - _dlpf_freq_icm_gyro = 0; + //_dlpf_freq_icm_gyro = 0; filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; } @@ -677,35 +414,35 @@ ICM20948::_set_dlpf_filter(uint16_t frequency_hz) choose next highest filter frequency available for accelerometer */ if (frequency_hz == 0) { - _dlpf_freq_icm_accel = 0; + //_dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } else if (frequency_hz <= 5) { - _dlpf_freq_icm_accel = 5; + //_dlpf_freq_icm_accel = 5; filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; } else if (frequency_hz <= 11) { - _dlpf_freq_icm_accel = 11; + //_dlpf_freq_icm_accel = 11; filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; } else if (frequency_hz <= 23) { - _dlpf_freq_icm_accel = 23; + //_dlpf_freq_icm_accel = 23; filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; } else if (frequency_hz <= 50) { - _dlpf_freq_icm_accel = 50; + //_dlpf_freq_icm_accel = 50; filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; } else if (frequency_hz <= 111) { - _dlpf_freq_icm_accel = 111; + //_dlpf_freq_icm_accel = 111; filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; } else if (frequency_hz <= 246) { - _dlpf_freq_icm_accel = 246; + //_dlpf_freq_icm_accel = 246; filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; } else { - _dlpf_freq_icm_accel = 0; + //_dlpf_freq_icm_accel = 0; filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; } @@ -764,15 +501,10 @@ ICM20948::select_register_bank(uint8_t bank) uint8_t ICM20948::read_reg(unsigned reg, uint32_t speed) { - uint8_t buf; - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + uint8_t buf{}; - } else { - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); - } + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); return buf; } @@ -780,35 +512,22 @@ ICM20948::read_reg(unsigned reg, uint32_t speed) uint8_t ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) { - uint8_t ret; - - if (buf == NULL) { return PX4_ERROR; } - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(start_reg)); - ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); - - } else { - ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + if (buf == NULL) { + return PX4_ERROR; } - return ret; + select_register_bank(REG_BANK(start_reg)); + return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } uint16_t ICM20948::read_reg16(unsigned reg) { - uint8_t buf[2]; + uint8_t buf[2] {}; // general register transfer at low clock speed - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); - - } else { - _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); - } + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); return (uint16_t)(buf[0] << 8) | buf[1]; } @@ -817,22 +536,14 @@ void ICM20948::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); - - } else { - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); - } + select_register_bank(REG_BANK(reg)); + _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); } void ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_reg(reg, val); @@ -841,9 +552,7 @@ ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) void ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_checked_reg(reg, val); @@ -868,27 +577,27 @@ ICM20948::set_accel_range(unsigned max_g_in) { uint8_t afs_sel; float lsb_per_g; - float max_accel_g; + //float max_accel_g; if (max_g_in > 8) { // 16g - AFS_SEL = 3 afs_sel = 3; lsb_per_g = 2048; - max_accel_g = 16; + //max_accel_g = 16; } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; - max_accel_g = 8; + //max_accel_g = 8; } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; - max_accel_g = 4; + //max_accel_g = 4; } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; - max_accel_g = 2; + //max_accel_g = 2; } switch (_whoami) { @@ -897,8 +606,7 @@ ICM20948::set_accel_range(unsigned max_g_in) break; } - _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); - _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; + _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); return OK; } @@ -909,15 +617,7 @@ ICM20948::start() /* make sure we are stopped first */ stop(); - /* discard any stale data in the buffers */ - if (!_magnetometer_only) { - _accel_reports->flush(); - _gyro_reports->flush(); - } - - _mag->_mag_reports->flush(); - - ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 10000); + ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); } void @@ -968,12 +668,7 @@ ICM20948::check_registers(void) // if the product_id is wrong then reset the // sensor completely - if (_whoami == ICM_WHOAMI_20948) { - // reset_mpu(); - } else { - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); - } + reset_mpu(); // after doing a reset we need to wait a long // time before we do any other register writes @@ -1044,7 +739,6 @@ bool ICM20948::check_duplicate(uint8_t *accel_data) void ICM20948::measure() { - if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete return; @@ -1067,11 +761,12 @@ ICM20948::measure() /* start measuring */ perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); /* * Fetch the full set of measurements from the MPU9250 in one pass */ - if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { + if ((!_magnetometer_only || _mag.is_passthrough()) && _register_wait == 0) { select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); @@ -1095,15 +790,15 @@ ICM20948::measure() # ifdef USE_I2C - if (_mag->is_passthrough()) { + if (_mag.is_passthrough()) { # endif - _mag->_measure(mpu_report.mag); + _mag._measure(timestamp_sample, mpu_report.mag); # ifdef USE_I2C } else { - _mag->measure(); + _mag.measure(); } # endif @@ -1155,6 +850,8 @@ ICM20948::measure() */ _last_temperature = (report.temp) / 333.87f + 21.0f; + _px4_accel.set_temperature(_last_temperature); + _px4_gyro.set_temperature(_last_temperature); /* * Convert and publish accelerometer and gyrometer data. @@ -1164,43 +861,30 @@ ICM20948::measure() /* * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation + * Swap axes and negate y */ - if (_whoami != ICM_WHOAMI_20948) { - /* - * Swap axes and negate y - */ - - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - } - /* - * Report buffers. - */ - sensor_accel_s arb; - sensor_gyro_s grb; + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); /* - * Adjust and scale results to m/s^2. + * Apply the swap */ - grb.timestamp = arb.timestamp = hrt_absolute_time(); + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1218,96 +902,8 @@ ICM20948::measure() */ /* NOTE: Axes have been swapped to match the board a few lines above. */ - - arb.x_raw = report.accel_x; - arb.y_raw = report.accel_y; - arb.z_raw = report.accel_z; - - float xraw_f = report.accel_x; - float yraw_f = report.accel_y; - float zraw_f = report.accel_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - arb.x = _accel_filter_x.apply(x_in_new); - arb.y = _accel_filter_y.apply(y_in_new); - arb.z = _accel_filter_z.apply(z_in_new); - - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; - - bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); - arb.x_integral = aval_integrated(0); - arb.y_integral = aval_integrated(1); - arb.z_integral = aval_integrated(2); - - arb.scaling = _accel_range_scale; - - arb.temperature = _last_temperature; - - /* return device ID */ - arb.device_id = _accel->_device_id.devid; - - grb.x_raw = report.gyro_x; - grb.y_raw = report.gyro_y; - grb.z_raw = report.gyro_z; - - xraw_f = report.gyro_x; - yraw_f = report.gyro_y; - zraw_f = report.gyro_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - - grb.x = _gyro_filter_x.apply(x_gyro_in_new); - grb.y = _gyro_filter_y.apply(y_gyro_in_new); - grb.z = _gyro_filter_z.apply(z_gyro_in_new); - - matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - matrix::Vector3f gval_integrated; - - bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); - grb.x_integral = gval_integrated(0); - grb.y_integral = gval_integrated(1); - grb.z_integral = gval_integrated(2); - - grb.scaling = _gyro_range_scale; - - grb.temperature = _last_temperature; - - /* return device ID */ - grb.device_id = _gyro->_device_id.devid; - - _accel_reports->force(&arb); - _gyro_reports->force(&grb); - - /* notify anyone waiting for data */ - if (accel_notify) { - _accel->poll_notify(POLLIN); - } - - if (gyro_notify) { - _gyro->parent_poll_notify(); - } - - if (accel_notify && !(_accel->_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - } - - if (gyro_notify && !(_gyro->_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); - } + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); } /* stop measuring */ @@ -1317,7 +913,6 @@ ICM20948::measure() void ICM20948::print_info() { - ::printf("Device type:%d\n", _whoami); perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); @@ -1326,11 +921,11 @@ ICM20948::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - ::printf("temperature: %.1f\n", (double)_last_temperature); if (!_magnetometer_only) { - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); - _mag->_mag_reports->print_info("mag queue"); + _px4_accel.print_status(); + _px4_gyro.print_status(); } + + _mag.print_status(); } diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h index 939b507346c3..abeead1d58c8 100644 --- a/src/drivers/imu/icm20948/icm20948.h +++ b/src/drivers/imu/icm20948/icm20948.h @@ -31,27 +31,24 @@ * ****************************************************************************/ -#include +#pragma once + +#include + +#include +#include + #include -#include -#include -#include + +#include +#include #include -#include -#include -#include -#include -#include -#include -#include #include -#include -#include +#include -#include "accel.h" -#include "gyro.h" -#include "mag.h" +#include +#include "ICM20948_mag.h" #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C @@ -366,21 +363,17 @@ extern int MPU9250_probe(device::Device *dev); typedef device::Device *(*ICM20948_constructor)(int, uint32_t, bool); class ICM20948_mag; -class ICM20948_accel; -class ICM20948_gyro; class ICM20948 : public px4::ScheduledWorkItem { public: - ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, - const char *path_mag, - enum Rotation rotation, + ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, bool magnetometer_only); virtual ~ICM20948(); virtual int init(); - uint8_t get_whoami(); + uint8_t get_whoami() { return _whoami; } /** * Diagnostics - print some basic information about the driver. @@ -393,40 +386,28 @@ class ICM20948 : public px4::ScheduledWorkItem virtual int probe(); - friend class ICM20948_accel; friend class ICM20948_mag; - friend class ICM20948_gyro; void Run() override; private: - ICM20948_accel *_accel; - ICM20948_gyro *_gyro; - ICM20948_mag *_mag; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + ICM20948_mag _mag; uint8_t _selected_bank; /* Remember selected memory bank to avoid polling / setting on each read/write */ bool _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ - unsigned _call_interval; - - ringbuffer::RingBuffer *_accel_reports; - - struct accel_calibration_s _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - ringbuffer::RingBuffer *_gyro_reports; - - struct gyro_calibration_s _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; + unsigned _call_interval{1000}; unsigned _dlpf_freq; unsigned _dlpf_freq_icm_gyro; unsigned _dlpf_freq_icm_accel; - unsigned _sample_rate; + unsigned _sample_rate{1000}; + perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; @@ -436,48 +417,32 @@ class ICM20948 : public px4::ScheduledWorkItem perf_counter_t _reset_retries; perf_counter_t _duplicates; - uint8_t _register_wait; - uint64_t _reset_wait; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - - Integrator _accel_int; - Integrator _gyro_int; - - enum Rotation _rotation; + uint8_t _register_wait{0}; + uint64_t _reset_wait{0}; // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#ifndef MAX -#define MAX(X,Y) ((X) > (Y) ? (X) : (Y)) -#endif - static constexpr int ICM20948_NUM_CHECKED_REGISTERS{15}; static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]; - const uint16_t *_checked_registers; + const uint16_t *_checked_registers{nullptr}; uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]; uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]; - unsigned _checked_next; - unsigned _num_checked_registers; + unsigned _checked_next{0}; + unsigned _num_checked_registers{0}; // last temperature reading for print_info() - float _last_temperature; + float _last_temperature{0.0f}; bool check_null_data(uint16_t *data, uint8_t size); bool check_duplicate(uint8_t *accel_data); // keep last accel reading for duplicate detection - uint8_t _last_accel_data[6]; - bool _got_duplicate; + uint8_t _last_accel_data[6] {}; + bool _got_duplicate{false}; /** * Start automatic measurement. @@ -619,7 +584,4 @@ class ICM20948 : public px4::ScheduledWorkItem */ void check_registers(void); - /* do not allow to copy this class due to pointer data members */ - ICM20948(const ICM20948 &); - ICM20948 operator=(const ICM20948 &); }; diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp index 732cb096bdce..9baba07575da 100644 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -37,7 +37,9 @@ * I2C interface for ICM20948 */ +#include #include +#include #include "icm20948.h" diff --git a/src/drivers/imu/icm20948/main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp similarity index 81% rename from src/drivers/imu/icm20948/main.cpp rename to src/drivers/imu/icm20948/icm20948_main.cpp index 642a6af53195..238c24a49e13 100644 --- a/src/drivers/imu/icm20948/main.cpp +++ b/src/drivers/imu/icm20948/icm20948_main.cpp @@ -39,11 +39,18 @@ * based on the mpu9250 driver */ +#include +#include +#include +#include +#include +#include +#include +#include + #include "icm20948.h" -#define ICM_DEVICE_PATH_ACCEL_EXT "/dev/icm20948_accel_ext" -#define ICM_DEVICE_PATH_GYRO_EXT "/dev/icm20948_gyro_ext" -#define ICM_DEVICE_PATH_MAG_EXT "/dev/icm20948_mag_ext" +#define ICM_DEVICE_PATH_EXT "/dev/icm20948_ext" /** driver 'main' command */ extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); } @@ -52,7 +59,7 @@ enum ICM20948_BUS { ICM20948_BUS_ALL = 0, ICM20948_BUS_I2C_INTERNAL, ICM20948_BUS_I2C_EXTERNAL, -// ICM20948_BUS_SPI_INTERNAL, + ICM20948_BUS_SPI_INTERNAL, // ICM20948_BUS_SPI_INTERNAL2, ICM20948_BUS_SPI_EXTERNAL }; @@ -69,20 +76,23 @@ namespace icm20948 struct icm20948_bus_option { enum ICM20948_BUS busid; - const char *accelpath; - const char *gyropath; - const char *magpath; + const char *path; ICM20948_constructor interface_constructor; bool magpassthrough; uint8_t busnum; uint32_t address; - ICM20948 *dev; + ICM20948 *dev; } bus_options[] = { + +#if defined(PX4_SPIDEV_ICM_20948) && defined(PX4_SPI_BUS_1) + { ICM20948_BUS_SPI_INTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_SPI_interface, true, PX4_SPI_BUS_1, PX4_SPIDEV_ICM_20948, nullptr }, +#endif + #if defined (USE_I2C) # if defined(PX4_I2C_BUS_EXPANSION) - { ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_ACCEL_EXT, ICM_DEVICE_PATH_GYRO_EXT, ICM_DEVICE_PATH_MAG_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, -#endif + { ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, +# endif #endif @@ -95,7 +105,6 @@ void start(enum ICM20948_BUS busid, enum Rotation rotation, bool external_bus, b bool start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid); void stop(enum ICM20948_BUS busid); -void reset(enum ICM20948_BUS busid); void info(enum ICM20948_BUS busid); void usage(); @@ -120,8 +129,6 @@ struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid) bool start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only) { - int fd = -1; - PX4_INFO("Bus probed: %d", bus.busid); if (bus.dev != nullptr) { @@ -154,8 +161,7 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external #endif - bus.dev = new ICM20948(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, - magnetometer_only); + bus.dev = new ICM20948(interface, mag_interface, bus.path, rotation, magnetometer_only); if (bus.dev == nullptr) { delete interface; @@ -171,33 +177,10 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external goto fail; } - /* - * Set the poll rate to default, starts automatic data collection. - * Doing this through the mag device for the time being - it's always there, even in magnetometer only mode. - * Using accel device for MPU6500. - */ - fd = open(bus.magpath, O_RDONLY); - - if (fd < 0) { - PX4_INFO("ioctl failed"); - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - - close(fd); - return true; fail: - if (fd >= 0) { - close(fd); - } - if (bus.dev != nullptr) { delete (bus.dev); bus.dev = nullptr; @@ -256,32 +239,6 @@ stop(enum ICM20948_BUS busid) exit(0); } -/** - * Reset the driver. - */ -void -reset(enum ICM20948_BUS busid) -{ - struct icm20948_bus_option &bus = find_bus(busid); - int fd = open(bus.accelpath, O_RDONLY); - - if (fd < 0) { - err(1, "failed "); - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - err(1, "driver reset failed"); - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - err(1, "driver poll restart failed"); - } - - close(fd); - - exit(0); -} - /** * Print a little info about the driver. */ @@ -304,7 +261,7 @@ info(enum ICM20948_BUS busid) void usage() { - PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + PX4_INFO("missing command: try 'start', 'info', 'stop',\n 'regdump', 'testerror'"); PX4_INFO("options:"); PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -I (i2c internal bus)"); @@ -383,13 +340,6 @@ icm20948_main(int argc, char *argv[]) icm20948::stop(busid); } - /* - * Reset the driver. - */ - if (!strcmp(verb, "reset")) { - icm20948::reset(busid); - } - /* * Print driver information. */ diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp index 87a04a945364..be38bf154765 100644 --- a/src/drivers/imu/icm20948/mag_i2c.cpp +++ b/src/drivers/imu/icm20948/mag_i2c.cpp @@ -38,7 +38,9 @@ */ #include "icm20948.h" -#include "mag.h" +#include "ICM20948_mag.h" + +#include #ifdef USE_I2C @@ -73,7 +75,7 @@ AK8963_I2C::AK8963_I2C(int bus) : int AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; @@ -108,4 +110,4 @@ AK8963_I2C::probe() return OK; } -#endif +#endif // USE_I2C diff --git a/src/drivers/imu/l3gd20/CMakeLists.txt b/src/drivers/imu/l3gd20/CMakeLists.txt index 6ca8ec793dec..6e090a9308e5 100644 --- a/src/drivers/imu/l3gd20/CMakeLists.txt +++ b/src/drivers/imu/l3gd20/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__l3gd20 MAIN l3gd20 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/l3gd20/l3gd20.cpp b/src/drivers/imu/l3gd20/l3gd20.cpp index 85aeb561d750..6cadddb99694 100644 --- a/src/drivers/imu/l3gd20/l3gd20.cpp +++ b/src/drivers/imu/l3gd20/l3gd20.cpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #define L3GD20_DEVICE_PATH "/dev/l3gd20" diff --git a/src/drivers/imu/lsm303d/CMakeLists.txt b/src/drivers/imu/lsm303d/CMakeLists.txt index 38d49e2955bb..2ad45e295fb0 100644 --- a/src/drivers/imu/lsm303d/CMakeLists.txt +++ b/src/drivers/imu/lsm303d/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__lsm303d MAIN lsm303d - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp index ab9abf39d380..819b162787d5 100644 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/imu/mpu6000/CMakeLists.txt b/src/drivers/imu/mpu6000/CMakeLists.txt index ec8c86153748..ad8aa89cf117 100644 --- a/src/drivers/imu/mpu6000/CMakeLists.txt +++ b/src/drivers/imu/mpu6000/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__mpu6000 MAIN mpu6000 - STACK_MAIN 1500 SRCS MPU6000.cpp MPU6000_I2C.cpp diff --git a/src/drivers/imu/mpu6000/MPU6000.hpp b/src/drivers/imu/mpu6000/MPU6000.hpp index 33817589ece3..a75a1716d16b 100644 --- a/src/drivers/imu/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/mpu6000/MPU6000.hpp @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/imu/mpu6000/mpu6000_main.cpp b/src/drivers/imu/mpu6000/mpu6000_main.cpp index bca90c3a463d..ec478fe7979c 100644 --- a/src/drivers/imu/mpu6000/mpu6000_main.cpp +++ b/src/drivers/imu/mpu6000/mpu6000_main.cpp @@ -78,6 +78,9 @@ struct mpu6000_bus_option { #if defined(PX4_SPIDEV_ICM_20602) && defined(PX4_SPI_BUS_SENSORS1) { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, ICM20602_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS1, false, NULL }, #endif +#if defined(PX4_SPIDEV_ICM_20602) && defined(PX4_SPI_BUS_1) + { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20602, ICM20602_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_1, false, NULL }, +#endif #ifdef PX4_SPIDEV_ICM_20608 { MPU6000_BUS_SPI_INTERNAL1, MPU_DEVICE_TYPE_ICM20608, ICM20608_DEVICE_PATH, &MPU6000_SPI_interface, PX4_SPI_BUS_SENSORS, false, NULL }, #endif diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt index 747decb21a1d..28ef1a59f84b 100644 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/mpu9250/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__mpu9250 MAIN mpu9250 - STACK_MAIN 1500 COMPILE_FLAGS SRCS AK8963_I2C.cpp diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 84e45cbf711c..65fe2bde2828 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 223b0e010e8a..5aa9f2c35b86 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/kinetis/CMakeLists.txt b/src/drivers/kinetis/CMakeLists.txt deleted file mode 100644 index a2fe213eb4d4..000000000000 --- a/src/drivers/kinetis/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016-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_library(drivers_arch - drv_hrt.c - drv_io_timer.c - drv_pwm_servo.c - drv_pwm_trigger.c - drv_input_capture.c - drv_led_pwm.cpp -) -add_dependencies(drivers_arch prebuild_targets) -target_link_libraries(drivers_arch PRIVATE drivers_board) -target_compile_options(drivers_arch PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/src/drivers/kinetis/adc/adc.cpp b/src/drivers/kinetis/adc/adc.cpp deleted file mode 100644 index e1888d431891..000000000000 --- a/src/drivers/kinetis/adc/adc.cpp +++ /dev/null @@ -1,520 +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 adc.cpp - * - * TODO:This is stubbed out leving the code intact to document the needed - * mechinsm for porting. - * - * Driver for the Kinetis ADC. - * - * This is a low-rate driver, designed for sampling things like voltages - * and so forth. It avoids the gross complexity of the NuttX ADC driver. - */ - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include -#include - -#if defined(ADC_CHANNELS) - -typedef uint32_t adc_chan_t; -#define ADC_TOTAL_CHANNELS 32 - -#define _REG(_addr) (*(volatile uint32_t *)(_addr)) - -/* ADC register accessors */ - -#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg)) - -#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */ -#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */ -#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */ -#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */ -#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */ -#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */ -#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */ -#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */ -#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */ -#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */ -#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */ -#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */ -#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */ -#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ - -class ADC : public device::CDev -{ -public: - ADC(adc_chan_t channels); - ~ADC(); - - virtual int init(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual ssize_t read(file *filp, char *buffer, size_t len); - -protected: - virtual int open_first(struct file *filp); - virtual int close_last(struct file *filp); - -private: - static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - - hrt_call _call; - perf_counter_t _sample_perf; - - adc_chan_t _channels; /**< bits set for channels */ - unsigned _channel_count; - adc_msg_s *_samples; /**< sample buffer */ - - orb_advert_t _to_system_power; - orb_advert_t _to_adc_report; - - /** work trampoline */ - static void _tick_trampoline(void *arg); - - /** worker function */ - void _tick(); - - /** - * Sample a single channel and return the measured value. - * - * @param channel The channel to sample. - * @return The sampled value, or 0xffff if - * sampling failed. - */ - uint16_t _sample(unsigned channel); - - // update system_power ORB topic, only on FMUv2 - void update_system_power(hrt_abstime now); - - void update_adc_report(hrt_abstime now); -}; - -ADC::ADC(adc_chan_t channels) : - CDev("adc", ADC0_DEVICE_PATH), - _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), - _channels(channels), - _channel_count(0), - _samples(nullptr), - _to_system_power(nullptr), - _to_adc_report(nullptr) -{ - _debug_enabled = true; - - /* always enable the temperature sensor */ - channels |= 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); - - /* allocate the sample array */ - for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { - if (channels & (1 << i)) { - _channel_count++; - } - } - - _samples = new adc_msg_s[_channel_count]; - - /* prefill the channel numbers in the sample array */ - - if (_samples != nullptr) { - unsigned index = 0; - - for (unsigned i = 0; i < ADC_TOTAL_CHANNELS; i++) { - if (channels & (1 << i)) { - _samples[index].am_channel = i; - _samples[index].am_data = 0; - index++; - } - } - } -} - -ADC::~ADC() -{ - if (_samples != nullptr) { - delete _samples; - } - - irqstate_t flags = px4_enter_critical_section(); - _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; - px4_leave_critical_section(flags); -} - -int -ADC::init() -{ - /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ - - irqstate_t flags = px4_enter_critical_section(); - - _REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1; - rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8; - rCFG2(1) = 0; - rSC2(1) = ADC_SC2_REFSEL_DEFAULT; - - px4_leave_critical_section(flags); - - /* Clear the CALF and begin the calibration */ - - rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF; - - while ((rSC1A(1) & ADC_SC1_COCO) == 0) { - usleep(100); - - if (rSC3(1) & ADC_SC3_CALF) { - return -1; - } - } - - /* dummy read to clear COCO of calibration */ - - int32_t r = rRA(1); - - /* Check the state of CALF at the end of calibration */ - - if (rSC3(1) & ADC_SC3_CALF) { - return -1; - } - - /* Calculate the calibration values for single ended positive */ - - r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ; - r = 0x8000U | (r >> 1U); - rPG(1) = r; - - /* Calculate the calibration values for double ended Negitive */ - - r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ; - r = 0x8000U | (r >> 1U); - rMG(1) = r; - - /* kick off a sample and wait for it to complete */ - hrt_abstime now = hrt_absolute_time(); - - rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP); - - while (!(rSC1A(1) & ADC_SC1_COCO)) { - - /* 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) { - DEVICE_LOG("sample timeout"); - return -1; - } - - break; - } - - - /* create the device node */ - return CDev::init(); -} - -int -ADC::ioctl(file *filp, int cmd, unsigned long arg) -{ - return -ENOTTY; -} - -ssize_t -ADC::read(file *filp, char *buffer, size_t len) -{ - const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - - if (len > maxsize) { - len = maxsize; - } - - /* block interrupts while copying samples to avoid racing with an update */ - irqstate_t flags = px4_enter_critical_section(); - memcpy(buffer, _samples, len); - px4_leave_critical_section(flags); - - return len; -} - -int -ADC::open_first(struct file *filp) -{ - /* get fresh data */ - _tick(); - - /* and schedule regular updates */ - hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this); - - return 0; -} - -int -ADC::close_last(struct file *filp) -{ - hrt_cancel(&_call); - return 0; -} - -void -ADC::_tick_trampoline(void *arg) -{ - (reinterpret_cast(arg))->_tick(); -} - -void -ADC::_tick() -{ - hrt_abstime now = hrt_absolute_time(); - - /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) { - _samples[i].am_data = _sample(_samples[i].am_channel); - } - - update_adc_report(now); - update_system_power(now); -} - -void -ADC::update_adc_report(hrt_abstime now) -{ - adc_report_s adc = {}; - adc.timestamp = now; - - unsigned max_num = _channel_count; - - if (max_num > (sizeof(adc.channel_id) / sizeof(adc.channel_id[0]))) { - max_num = (sizeof(adc.channel_id) / sizeof(adc.channel_id[0])); - } - - for (unsigned i = 0; i < max_num; i++) { - adc.channel_id[i] = _samples[i].am_channel; - adc.channel_value[i] = _samples[i].am_data * 3.3f / 4096.0f; - } - - int instance; - orb_publish_auto(ORB_ID(adc_report), &_to_adc_report, &adc, &instance, ORB_PRIO_HIGH); -} - -void -ADC::update_system_power(hrt_abstime now) -{ -#if defined (BOARD_ADC_USB_CONNECTED) - system_power_s system_power = {}; - system_power.timestamp = now; - - system_power.voltage5v_v = 0; - -#if defined(ADC_5V_RAIL_SENSE) - - for (unsigned i = 0; i < _channel_count; i++) { - - if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { - // it is 2:1 scaled - system_power.voltage5v_v = _samples[i].am_data * (6.6f / 4096.0f); - } - } - -#endif - - - /* Note once the board_config.h provides BOARD_ADC_USB_CONNECTED, - * It must provide the true logic GPIO BOARD_ADC_xxxx macros. - */ - // these are not ADC related, but it is convenient to - // publish these to the same topic - - system_power.usb_connected = BOARD_ADC_USB_CONNECTED; - /* If provided used the Valid signal from HW*/ -#if defined(BOARD_ADC_USB_VALID) - system_power.usb_valid = BOARD_ADC_USB_VALID; -#else - /* If not provided then use connected */ - system_power.usb_valid = system_power.usb_connected; -#endif - - system_power.brick_valid = BOARD_ADC_BRICK_VALID; - system_power.servo_valid = BOARD_ADC_SERVO_VALID; - - // OC pins are active low - system_power.periph_5v_oc = BOARD_ADC_PERIPH_5V_OC; - system_power.hipower_5v_oc = BOARD_ADC_HIPOWER_5V_OC; - - /* lazily publish */ - if (_to_system_power != nullptr) { - orb_publish(ORB_ID(system_power), _to_system_power, &system_power); - - } else { - _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); - } - -#endif // BOARD_ADC_USB_CONNECTED -} - -uint16_t -ADC::_sample(unsigned channel) -{ - perf_begin(_sample_perf); - - /* clear any previous COCC */ - uint16_t result = rRA(1); - - /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ - - rSC1A(1) = ADC_SC1_ADCH(channel); - - /* wait for the conversion to complete */ - hrt_abstime now = hrt_absolute_time(); - - while (!(rSC1A(1) & ADC_SC1_COCO)) { - - /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ - if ((hrt_absolute_time() - now) > 10) { - DEVICE_LOG("sample timeout"); - return 0xffff; - } - } - - /* read the result and clear EOC */ - result = rRA(1); - - perf_end(_sample_perf); - return result; -} - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int adc_main(int argc, char *argv[]); - -namespace -{ -ADC *g_adc; - -void -test(void) -{ - - int fd = open(ADC0_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - PX4_ERR("can't open ADC device %d", errno); - exit(1); - } - - for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[ADC_TOTAL_CHANNELS]; - ssize_t count = read(fd, data, sizeof(data)); - - if (count < 0) { - PX4_ERR("read error"); - exit(1); - } - - unsigned channels = count / sizeof(data[0]); - - for (unsigned j = 0; j < channels; j++) { - printf("%d: %u ", data[j].am_channel, data[j].am_data); - } - - printf("\n"); - usleep(500000); - } - - exit(0); -} -} - -int -adc_main(int argc, char *argv[]) -{ - if (g_adc == nullptr) { - /* XXX this hardcodes the default channel set for the board in board_config.h - should be configurable */ - g_adc = new ADC(ADC_CHANNELS); - - if (g_adc == nullptr) { - PX4_ERR("couldn't allocate the ADC driver"); - exit(1); - } - - if (g_adc->init() != OK) { - delete g_adc; - PX4_ERR("ADC init failed"); - exit(1); - } - } - - if (argc > 1) { - if (!strcmp(argv[1], "test")) { - test(); - } - } - - exit(0); -} -#endif diff --git a/src/drivers/lights/CMakeLists.txt b/src/drivers/lights/CMakeLists.txt index 8cf09d988556..f80c47a6fbd4 100644 --- a/src/drivers/lights/CMakeLists.txt +++ b/src/drivers/lights/CMakeLists.txt @@ -32,8 +32,6 @@ ############################################################################ add_subdirectory(blinkm) -add_subdirectory(oreoled) -add_subdirectory(pca8574) add_subdirectory(rgbled) add_subdirectory(rgbled_ncp5623c) add_subdirectory(rgbled_pwm) diff --git a/src/drivers/lights/blinkm/blinkm.cpp b/src/drivers/lights/blinkm/blinkm.cpp index 94329bb5251c..27f28f344d0f 100644 --- a/src/drivers/lights/blinkm/blinkm.cpp +++ b/src/drivers/lights/blinkm/blinkm.cpp @@ -99,7 +99,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/drivers/lights/blinkm/parameters.c b/src/drivers/lights/blinkm/parameters.c new file mode 100644 index 000000000000..592743c9cd51 --- /dev/null +++ b/src/drivers/lights/blinkm/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * BlinkM LED + * + * @reboot_required true + * + * @boolean + * @group Peripheral + */ +PARAM_DEFINE_INT32(LIGHT_EN_BLINKM, 0); diff --git a/src/drivers/lights/oreoled/oreoled.cpp b/src/drivers/lights/oreoled/oreoled.cpp deleted file mode 100644 index 0b546cc88c64..000000000000 --- a/src/drivers/lights/oreoled/oreoled.cpp +++ /dev/null @@ -1,2003 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Randy Mackay - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (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 oreoled.cpp - * - * Driver for oreoled ESCs found in solo, connected via I2C. - * - */ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#define OREOLED_NUM_LEDS 4 ///< maximum number of LEDs the oreo led driver can support -#define OREOLED_BASE_I2C_ADDR 0x68 ///< base i2c address (7-bit) -#define OPEOLED_I2C_RETRYCOUNT 2 ///< i2c retry count -#define OREOLED_TIMEOUT_USEC 2000000U ///< timeout looking for oreoleds 2 seconds after startup -#define OREOLED_GENERALCALL_US 4000000U ///< general call sent every 4 seconds -#define OREOLED_GENERALCALL_CMD 0x00 ///< general call command sent at regular intervals - -#define OREOLED_STARTUP_INTERVAL_US (1000000U / 10U) ///< time in microseconds, measure at 10hz -#define OREOLED_UPDATE_INTERVAL_US (1000000U / 50U) ///< time in microseconds, measure at 10hz - -#define OREOLED_CMD_QUEUE_SIZE 10 ///< up to 10 messages can be queued up to send to the LEDs - -class OREOLED : public device::I2C, public px4::ScheduledWorkItem -{ -public: - OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate); - virtual ~OREOLED(); - - virtual int init(); - virtual int probe(); - virtual int info(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /* send general call on I2C bus to syncronise all LEDs */ - int send_general_call(); - - /* send cmd to an LEDs (used for testing only) */ - int send_cmd(oreoled_cmd_t sb); - - /* returns true once the driver finished bootloading and ready for commands */ - bool is_ready(); - -private: - - /** - * Start periodic updates to the LEDs - */ - void start(); - - /** - * Stop periodic updates to the LEDs - */ - void stop(); - - /** - * update the colours displayed by the LEDs - */ - void Run() override; - - int bootloader_app_reset(int led_num); - int bootloader_app_ping(int led_num); - uint16_t bootloader_inapp_checksum(int led_num); - int bootloader_ping(int led_num); - uint8_t bootloader_version(int led_num); - uint16_t bootloader_app_version(int led_num); - uint16_t bootloader_app_checksum(int led_num); - int bootloader_set_colour(int led_num, uint8_t red, uint8_t green); - int bootloader_flash(int led_num); - int bootloader_boot(int led_num); - uint16_t bootloader_fw_checksum(void); - int bootloader_coerce_healthy(void); - - /* internal variables */ - bool _healthy[OREOLED_NUM_LEDS]; ///< health of each LED - bool _in_boot[OREOLED_NUM_LEDS]; ///< true for each LED that is in bootloader mode - uint8_t _num_healthy; ///< number of healthy LEDs - uint8_t _num_inboot; ///< number of LEDs in bootloader - ringbuffer::RingBuffer *_cmd_queue; ///< buffer of commands to send to LEDs - uint64_t _last_gencall; - uint64_t _start_time; ///< system time we first attempt to communicate with battery - bool _autoupdate; ///< true if the driver should update all LEDs - bool _alwaysupdate; ///< true if the driver should update all LEDs - bool _is_bootloading; ///< true if a bootloading operation is in progress - bool _is_ready; ///< set to true once the driver has completly initialised - uint16_t _fw_checksum; ///< the current 16bit XOR checksum of the built in oreoled firmware binary - - /* performance checking */ - perf_counter_t _call_perf; - perf_counter_t _gcall_perf; - perf_counter_t _probe_perf; - perf_counter_t _comms_errors; - perf_counter_t _reply_errors; -}; - -/* for now, we only support one OREOLED */ -namespace -{ -OREOLED *g_oreoled = nullptr; -} - -void oreoled_usage(); - -extern "C" __EXPORT int oreoled_main(int argc, char *argv[]); - -/* constructor */ -OREOLED::OREOLED(int bus, int i2c_addr, bool autoupdate, bool alwaysupdate) : - I2C("oreoled", OREOLED0_DEVICE_PATH, bus, i2c_addr, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _num_healthy(0), - _num_inboot(0), - _cmd_queue(nullptr), - _last_gencall(0), - _autoupdate(autoupdate), - _alwaysupdate(alwaysupdate), - _is_bootloading(false), - _is_ready(false), - _fw_checksum(0x0000), - _call_perf(perf_alloc(PC_ELAPSED, "oreoled_call")), - _gcall_perf(perf_alloc(PC_ELAPSED, "oreoled_gcall")), - _probe_perf(perf_alloc(PC_ELAPSED, "oreoled_probe")), - _comms_errors(perf_alloc(PC_COUNT, "oreoled_comms_errors")), - _reply_errors(perf_alloc(PC_COUNT, "oreoled_reply_errors")) -{ - /* initialise to unhealthy */ - memset(_healthy, 0, sizeof(_healthy)); - - /* initialise to in application */ - memset(_in_boot, 0, sizeof(_in_boot)); - - /* capture startup time */ - _start_time = hrt_absolute_time(); -} - -/* destructor */ -OREOLED::~OREOLED() -{ - /* make sure we are truly inactive */ - stop(); - - /* clear command queue */ - if (_cmd_queue != nullptr) { - delete _cmd_queue; - } - - /* free perf counters */ - perf_free(_call_perf); - perf_free(_gcall_perf); - perf_free(_probe_perf); - perf_free(_comms_errors); - perf_free(_reply_errors); -} - -int -OREOLED::init() -{ - int ret; - - /* initialise I2C bus */ - ret = I2C::init(); - - if (ret != OK) { - return ret; - } - - /* allocate command queue */ - _cmd_queue = new ringbuffer::RingBuffer(OREOLED_CMD_QUEUE_SIZE, sizeof(oreoled_cmd_t)); - - if (_cmd_queue == nullptr) { - return ENOTTY; - - } else { - /* start work queue */ - start(); - } - - return OK; -} - -int -OREOLED::probe() -{ - /* set retry count */ - _retries = OPEOLED_I2C_RETRYCOUNT; - - /* always return true */ - return OK; -} - -int -OREOLED::info() -{ - /* print health info on each LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i]) { - DEVICE_LOG("oreo %u: BAD", (unsigned)i); - - } else { - DEVICE_LOG("oreo %u: OK", (unsigned)i); - } - } - - /* display perf info */ - perf_print_counter(_call_perf); - perf_print_counter(_gcall_perf); - perf_print_counter(_probe_perf); - perf_print_counter(_comms_errors); - perf_print_counter(_reply_errors); - - return OK; -} - -void -OREOLED::start() -{ - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void -OREOLED::stop() -{ - ScheduleClear(); -} - -void -OREOLED::Run() -{ - /* check time since startup */ - uint64_t now = hrt_absolute_time(); - bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_USEC); - - /* prepare the response buffer */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* during startup period keep searching for unhealthy LEDs */ - if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) { - /* prepare command to turn off LED */ - /* add two bytes of pre-amble to for higher signal to noise ratio */ - uint8_t msg[] = {0xAA, 0x55, OREOLED_PATTERN_OFF, 0x00}; - - /* attempt to contact each unhealthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i]) { - perf_begin(_probe_perf); - - /* set I2C address */ - set_device_address(OREOLED_BASE_I2C_ADDR + i); - - /* Calculate XOR CRC and append to the i2c write data */ - msg[sizeof(msg) - 1] = OREOLED_BASE_I2C_ADDR + i; - - for (uint8_t j = 0; j < sizeof(msg) - 1; j++) { - msg[sizeof(msg) - 1] ^= msg[j]; - } - - /* send I2C command */ - if (transfer(msg, sizeof(msg), reply, 3) == OK) { - if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1]) { - DEVICE_LOG("oreoled %u ok - in bootloader", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - /* If slaves are in application record that so we can reset if we need to bootload */ - /* This additional check is required for LED firmwares below v1.3 and can be - deprecated once all LEDs in the wild have firmware >= v1.3 */ - if (bootloader_ping(i) == OK) { - _in_boot[i] = true; - _num_inboot++; - } - - /* Check for a reply with a checksum offset of 1, - which indicates a response from firmwares >= 1.3 */ - - } else if (reply[1] == OREOLED_BASE_I2C_ADDR + i && - reply[2] == msg[sizeof(msg) - 1] + 1) { - DEVICE_LOG("oreoled %u ok - in application", (unsigned)i); - _healthy[i] = true; - _num_healthy++; - - } else { - DEVICE_LOG("oreo reply errors: %u", (unsigned)_reply_errors); - perf_count(_reply_errors); - } - - } else { - perf_count(_comms_errors); - } - - perf_end(_probe_perf); - } - } - - /* schedule another attempt in 0.1 sec */ - ScheduleDelayed(OREOLED_STARTUP_INTERVAL_US); - - return; - - } else if (_alwaysupdate) { - /* reset each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - bootloader_app_reset(i); - } - } - - /* attempt to update each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* flash the new firmware */ - bootloader_flash(i); - } - } - - /* boot each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* boot the application */ - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - - /* mandatory updating has finished */ - _alwaysupdate = false; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(OREOLED_UPDATE_INTERVAL_US); - return; - - } else if (_autoupdate) { - /* check booted oreoleds to see if the app can report it's checksum (release versions >= v1.2) */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* put any out of date oreoleds into bootloader mode */ - /* being in bootloader mode signals to be code below that the will likey need updating */ - if (bootloader_inapp_checksum(i) != bootloader_fw_checksum()) { - bootloader_app_reset(i); - } - } - } - - /* reset all healthy oreoleds if the number of outdated oreoled's is > 0 */ - /* this is done for consistency, so if only one oreoled is updating, all LEDs show the same behaviour */ - /* otherwise a single oreoled could appear broken or failed. */ - if (_num_inboot > 0) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && !_in_boot[i]) { - /* reset the LED if it's not in the bootloader */ - /* (this happens during a pixhawk OTA update, since the LEDs stay powered) */ - bootloader_app_reset(i); - } - } - - /* update each outdated and healthy LED in bootloader mode */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* only flash LEDs with an old version of the applictioon */ - if (bootloader_app_checksum(i) != bootloader_fw_checksum()) { - /* flash the new firmware */ - bootloader_flash(i); - } - } - } - - /* boot each healthy LED */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i] && _in_boot[i]) { - /* boot the application */ - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - } - - /* auto updating has finished */ - _autoupdate = false; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(OREOLED_UPDATE_INTERVAL_US); - return; - - } else if (_num_inboot > 0) { - /* boot any LEDs which are in still in bootloader mode */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_in_boot[i]) { - bootloader_boot(i); - } - } - - /* coerce LEDs with startup issues to be healthy again */ - bootloader_coerce_healthy(); - - /* ensure we don't get stuck in a loop */ - _num_inboot = 0; - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(OREOLED_UPDATE_INTERVAL_US); - return; - - } else if (!_is_ready) { - /* indicate a ready state since startup has finished */ - _is_ready = true; - } - - /* get next command from queue */ - oreoled_cmd_t next_cmd; - - while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) { - /* send valid messages to healthy LEDs */ - if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num] - && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) { - /* start performance timer */ - perf_begin(_call_perf); - - /* set I2C address */ - set_device_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num); - - /* Calculate XOR CRC and append to the i2c write data */ - uint8_t next_cmd_xor = OREOLED_BASE_I2C_ADDR + next_cmd.led_num; - - for (uint8_t i = 0; i < next_cmd.num_bytes; i++) { - next_cmd_xor ^= next_cmd.buff[i]; - } - - next_cmd.buff[next_cmd.num_bytes++] = next_cmd_xor; - - /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(next_cmd.buff, next_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + next_cmd.led_num) && reply[2] == next_cmd_xor) { - /* slave returned a valid response */ - break; - - } else { - perf_count(_reply_errors); - } - - } else { - perf_count(_comms_errors); - } - } - - perf_end(_call_perf); - } - } - - /* send general call every 4 seconds, if we aren't bootloading*/ - if (!_is_bootloading && ((now - _last_gencall) > OREOLED_GENERALCALL_US)) { - perf_begin(_gcall_perf); - send_general_call(); - perf_end(_gcall_perf); - } - - /* schedule a fresh cycle call when the command is sent */ - ScheduleDelayed(OREOLED_UPDATE_INTERVAL_US); -} - -int -OREOLED::bootloader_app_reset(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - /* send a reset */ - boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - boot_cmd.buff[1] = OREOLED_PARAM_RESET; - boot_cmd.buff[2] = OEROLED_RESET_NONCE; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - /* slave returned a valid response */ - ret = OK; - /* set this LED as being in boot mode now */ - _in_boot[led_num] = true; - _num_inboot++; - break; - } - } - } - - /* Allow time for the LED to reboot */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_app_ping(int led_num) -{ - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - /* send a pattern off command */ - boot_cmd.buff[0] = 0xAA; - boot_cmd.buff[1] = 0x55; - boot_cmd.buff[2] = OREOLED_PATTERN_OFF; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* send I2C command with a retry limit */ - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { - if (transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 3) == OK) { - if (reply[1] == (OREOLED_BASE_I2C_ADDR + boot_cmd.led_num) && - reply[2] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - /* slave returned a valid response */ - ret = OK; - break; - } - } - } - - return ret; -} - -uint16_t -OREOLED::bootloader_inapp_checksum(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - boot_cmd.buff[1] = OREOLED_PARAM_APP_CHECKSUM; - boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 3; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_PARAM_APP_CHECKSUM && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app checksum OK from LED %i", boot_cmd.led_num); - warnx("bl app checksum msb: 0x%x", reply[3]); - warnx("bl app checksum lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app checksum retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app checksum failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_ping(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_PING; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_PING && - reply[3] == OREOLED_BOOT_CMD_PING_NONCE && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl ping OK from LED %i", boot_cmd.led_num); - ret = OK; - break; - - } else { - warnx("bl ping FAIL from LED %i", boot_cmd.led_num); - warnx("bl ping response ADDR: 0x%x", reply[1]); - warnx("bl ping response CMD: 0x%x", reply[2]); - warnx("bl ping response NONCE: 0x%x", reply[3]); - warnx("bl ping response XOR: 0x%x", reply[4]); - - if (retry > 1) { - warnx("bl ping retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl ping failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint8_t -OREOLED::bootloader_version(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint8_t ret = 0x00; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_BL_VER; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 5); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BL_VER && - reply[3] == OREOLED_BOOT_SUPPORTED_VER && - reply[4] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl ver from LED %i = %i", boot_cmd.led_num, reply[3]); - ret = reply[3]; - break; - - } else { - warnx("bl ver response ADDR: 0x%x", reply[1]); - warnx("bl ver response CMD: 0x%x", reply[2]); - warnx("bl ver response VER: 0x%x", reply[3]); - warnx("bl ver response XOR: 0x%x", reply[4]); - - if (retry > 1) { - warnx("bl ver retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl ver failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint16_t -OREOLED::bootloader_app_version(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_VER; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_VER && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app version OK from LED %i", boot_cmd.led_num); - warnx("bl app version msb: 0x%x", reply[3]); - warnx("bl app version lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app version FAIL from LED %i", boot_cmd.led_num); - warnx("bl app version response ADDR: 0x%x", reply[1]); - warnx("bl app version response CMD: 0x%x", reply[2]); - warnx("bl app version response VER H: 0x%x", reply[3]); - warnx("bl app version response VER L: 0x%x", reply[4]); - warnx("bl app version response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app version retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app version failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -uint16_t -OREOLED::bootloader_app_checksum(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - uint16_t ret = 0x0000; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_APP_CRC; - boot_cmd.buff[1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 2; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 6); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_APP_CRC && - reply[5] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl app checksum OK from LED %i", boot_cmd.led_num); - warnx("bl app checksum msb: 0x%x", reply[3]); - warnx("bl app checksum lsb: 0x%x", reply[4]); - ret = ((reply[3] << 8) | reply[4]); - break; - - } else { - warnx("bl app checksum FAIL from LED %i", boot_cmd.led_num); - warnx("bl app checksum response ADDR: 0x%x", reply[1]); - warnx("bl app checksum response CMD: 0x%x", reply[2]); - warnx("bl app checksum response VER H: 0x%x", reply[3]); - warnx("bl app checksum response VER L: 0x%x", reply[4]); - warnx("bl app checksum response XOR: 0x%x", reply[5]); - - if (retry > 1) { - warnx("bl app checksum retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app checksum failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_set_colour(int led_num, uint8_t red, uint8_t green) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_SET_COLOUR; - boot_cmd.buff[1] = red; - boot_cmd.buff[2] = green; - boot_cmd.buff[3] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 4; - - for (uint8_t j = 0; j < boot_cmd.num_bytes - 1; j++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[j]; - } - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_SET_COLOUR && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl set colour OK from LED %i", boot_cmd.led_num); - ret = OK; - break; - - } else { - warnx("bl set colour FAIL from LED %i", boot_cmd.led_num); - warnx("bl set colour response ADDR: 0x%x", reply[1]); - warnx("bl set colour response CMD: 0x%x", reply[2]); - warnx("bl set colour response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl app colour retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl app colour failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - _is_bootloading = false; - return ret; -} - -int -OREOLED::bootloader_flash(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - /* Open the bootloader file */ - int fd = ::open(OREOLED_FW_FILE, O_RDONLY); - - /* check for error opening the file */ - if (fd < 0) { - return -1; - } - - struct stat s; - - /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) { - ::close(fd); - return -1; - } - - uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; - - /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { - ::close(fd); - return -1; - } - - uint8_t *buf = new uint8_t[s.st_size]; - - /* check that the buffer has been allocated */ - if (buf == NULL) { - ::close(fd); - return -1; - } - - /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) { - ::close(fd); - delete[] buf; - return -1; - } - - ::close(fd); - - /* Grab the version bytes from the binary */ - uint8_t version_major = buf[0]; - uint8_t version_minor = buf[1]; - - /* calculate flash pages (rounded up to nearest integer) */ - uint8_t flash_pages = ((fw_length + 64 - 1) / 64); - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - - /* Loop through flash pages */ - for (uint8_t page_idx = 0; page_idx < flash_pages; page_idx++) { - - /* Send the first half of the 64 byte flash page */ - memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); - boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_A; - boot_cmd.buff[1] = page_idx; - memcpy(boot_cmd.buff + 2, buf + (page_idx * 64) + OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32 + 2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32 + 3; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_A && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl flash %ia OK for LED %i", page_idx, boot_cmd.led_num); - break; - - } else { - warnx("bl flash %ia FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ia response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ia response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ia response XOR: 0x%x", page_idx, reply[3]); - - if (retry > 1) { - warnx("bl flash %ia retrying LED %i", page_idx, boot_cmd.led_num); - - } else { - warnx("bl flash %ia failed on LED %i", page_idx, boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* Send the second half of the 64 byte flash page */ - memset(boot_cmd.buff, 0, sizeof(boot_cmd.buff)); - boot_cmd.buff[0] = OREOLED_BOOT_CMD_WRITE_FLASH_B; - memcpy(boot_cmd.buff + 1, buf + (page_idx * 64) + 32 + OREOLED_FW_FILE_HEADER_LENGTH, 32); - boot_cmd.buff[32 + 1] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 32 + 2; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write+Read */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_WRITE_FLASH_B && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl flash %ib OK for LED %i", page_idx, boot_cmd.led_num); - break; - - } else { - warnx("bl flash %ib FAIL for LED %i", page_idx, boot_cmd.led_num); - warnx("bl flash %ib response ADDR: 0x%x", page_idx, reply[1]); - warnx("bl flash %ib response CMD: 0x%x", page_idx, reply[2]); - warnx("bl flash %ib response XOR: 0x%x", page_idx, reply[3]); - - if (retry > 1) { - warnx("bl flash %ib retrying LED %i", page_idx, boot_cmd.led_num); - - } else { - errx(1, "bl flash %ib failed on LED %i", page_idx, boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* Sleep to allow flash to write */ - /* Wait extra long on the first write, to allow time for EEPROM updates */ - if (page_idx == 0) { - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - } else { - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000); - } - } - - uint16_t app_checksum = bootloader_fw_checksum(); - - /* Flash writes must have succeeded so finalise the flash */ - boot_cmd.buff[0] = OREOLED_BOOT_CMD_FINALISE_FLASH; - boot_cmd.buff[1] = version_major; - boot_cmd.buff[2] = version_minor; - boot_cmd.buff[3] = (uint8_t)(fw_length >> 8); - boot_cmd.buff[4] = (uint8_t)(fw_length & 0xFF); - boot_cmd.buff[5] = (uint8_t)(app_checksum >> 8); - boot_cmd.buff[6] = (uint8_t)(app_checksum & 0xFF); - boot_cmd.buff[7] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 8; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } - - /* Try to finalise for twice the amount of normal retries */ - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES * 2; retry > 0; retry--) { - /* Send the I2C Write */ - memset(reply, 0, sizeof(reply)); - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_FINALISE_FLASH && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl finalise OK from LED %i", boot_cmd.led_num); - break; - - } else { - warnx("bl finalise response ADDR: 0x%x", reply[1]); - warnx("bl finalise response CMD: 0x%x", reply[2]); - warnx("bl finalise response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl finalise retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl finalise failed on LED %i", boot_cmd.led_num); - delete[] buf; - return -1; - } - } - } - - /* allow time for flash to finalise */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - /* clean up file buffer */ - delete[] buf; - - _is_bootloading = false; - return 1; -} - -int -OREOLED::bootloader_boot(int led_num) -{ - _is_bootloading = true; - oreoled_cmd_t boot_cmd; - boot_cmd.led_num = led_num; - - int ret = -1; - - /* Set the current address */ - set_device_address(OREOLED_BASE_I2C_ADDR + boot_cmd.led_num); - - boot_cmd.buff[0] = OREOLED_BOOT_CMD_BOOT_APP; - boot_cmd.buff[1] = OREOLED_BOOT_CMD_BOOT_NONCE; - boot_cmd.buff[2] = OREOLED_BASE_I2C_ADDR + boot_cmd.led_num; - boot_cmd.num_bytes = 3; - - for (uint8_t k = 0; k < boot_cmd.num_bytes - 1; k++) { - boot_cmd.buff[boot_cmd.num_bytes - 1] ^= boot_cmd.buff[k]; - } - - for (uint8_t retry = OEROLED_BOOT_COMMAND_RETRIES; retry > 0; retry--) { - /* Send the I2C Write */ - uint8_t reply[OREOLED_CMD_READ_LENGTH_MAX]; - transfer(boot_cmd.buff, boot_cmd.num_bytes, reply, 4); - - /* Check the response */ - if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_APP && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl boot OK from LED %i", boot_cmd.led_num); - /* decrement the inboot counter so we don't get confused */ - _in_boot[led_num] = false; - _num_inboot--; - ret = OK; - break; - - } else if (reply[1] == OREOLED_BASE_I2C_ADDR + boot_cmd.led_num && - reply[2] == OREOLED_BOOT_CMD_BOOT_NONCE && - reply[3] == boot_cmd.buff[boot_cmd.num_bytes - 1]) { - warnx("bl boot error from LED %i: no app", boot_cmd.led_num); - break; - - } else { - warnx("bl boot response ADDR: 0x%x", reply[1]); - warnx("bl boot response CMD: 0x%x", reply[2]); - warnx("bl boot response XOR: 0x%x", reply[3]); - - if (retry > 1) { - warnx("bl boot retrying LED %i", boot_cmd.led_num); - - } else { - warnx("bl boot failed on LED %i", boot_cmd.led_num); - break; - } - } - } - - /* allow time for the LEDs to boot */ - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - usleep(OREOLED_BOOT_FLASH_WAITMS * 1000 * 10); - - _is_bootloading = false; - return ret; -} - -uint16_t -OREOLED::bootloader_fw_checksum(void) -{ - /* Calculate the 16 bit XOR checksum of the firmware on the first call of this function */ - if (_fw_checksum == 0x0000) { - /* Open the bootloader file */ - int fd = ::open(OREOLED_FW_FILE, O_RDONLY); - - /* check for error opening the file */ - if (fd < 0) { - return -1; - } - - struct stat s; - - /* attempt to stat the file */ - if (stat(OREOLED_FW_FILE, &s) != 0) { - ::close(fd); - return -1; - } - - uint16_t fw_length = s.st_size - OREOLED_FW_FILE_HEADER_LENGTH; - - /* sanity-check file size */ - if (fw_length > OREOLED_FW_FILE_SIZE_LIMIT) { - ::close(fd); - return -1; - } - - uint8_t *buf = new uint8_t[s.st_size]; - - /* check that the buffer has been allocated */ - if (buf == NULL) { - ::close(fd); - return -1; - } - - /* check that the firmware can be read into the buffer */ - if (::read(fd, buf, s.st_size) != s.st_size) { - ::close(fd); - delete[] buf; - return -1; - } - - ::close(fd); - - /* Calculate a 16 bit XOR checksum of the flash */ - /* Skip the first two bytes which are the version information, plus - the next two bytes which are modified by the bootloader */ - uint16_t app_checksum = 0x0000; - - for (uint16_t j = 2 + OREOLED_FW_FILE_HEADER_LENGTH; j < s.st_size; j += 2) { - app_checksum ^= (buf[j] << 8) | buf[j + 1]; - } - - delete[] buf; - - warnx("fw length = %i", fw_length); - warnx("fw checksum = %i", app_checksum); - - /* Store the checksum so it's only calculated once */ - _fw_checksum = app_checksum; - } - - return _fw_checksum; -} - -int -OREOLED::bootloader_coerce_healthy(void) -{ - int ret = -1; - - /* check each unhealthy LED */ - /* this re-checks "unhealthy" LEDs as they can sometimes power up with the wrong ID, */ - /* but will have the correct ID once they jump to the application and be healthy again */ - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (!_healthy[i] && bootloader_app_ping(i) == OK) { - /* mark as healthy */ - _healthy[i] = true; - _num_healthy++; - ret = OK; - } - } - - return ret; -} - -int -OREOLED::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = -ENODEV; - oreoled_cmd_t new_cmd; - - switch (cmd) { - case OREOLED_SET_RGB: - /* set the specified color */ - new_cmd.led_num = ((oreoled_rgbset_t *) arg)->instance; - new_cmd.buff[0] = ((oreoled_rgbset_t *) arg)->pattern; - new_cmd.buff[1] = OREOLED_PARAM_BIAS_RED; - new_cmd.buff[2] = ((oreoled_rgbset_t *) arg)->red; - new_cmd.buff[3] = OREOLED_PARAM_BIAS_GREEN; - new_cmd.buff[4] = ((oreoled_rgbset_t *) arg)->green; - new_cmd.buff[5] = OREOLED_PARAM_BIAS_BLUE; - new_cmd.buff[6] = ((oreoled_rgbset_t *) arg)->blue; - new_cmd.num_bytes = 7; - - /* special handling for request to set all instances rgb values */ - if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { - /* request to set individual instance's rgb value */ - if (_healthy[new_cmd.led_num]) { - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - - case OREOLED_RUN_MACRO: - /* run a macro */ - new_cmd.led_num = ((oreoled_macrorun_t *) arg)->instance; - new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - new_cmd.buff[1] = OREOLED_PARAM_MACRO; - new_cmd.buff[2] = ((oreoled_macrorun_t *) arg)->macro; - new_cmd.num_bytes = 3; - - /* special handling for request to set all instances */ - if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { - /* request to set individual instance's rgb value */ - if (_healthy[new_cmd.led_num]) { - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - - case OREOLED_SEND_RESET: - /* send a reset */ - new_cmd.led_num = OREOLED_ALL_INSTANCES; - new_cmd.buff[0] = OREOLED_PATTERN_PARAMUPDATE; - new_cmd.buff[1] = OREOLED_PARAM_RESET; - new_cmd.buff[2] = OEROLED_RESET_NONCE; - new_cmd.num_bytes = 3; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - warnx("sending a reset... to %i", i); - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_PING: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_ping(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_FLASH: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_flash(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_VER: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_app_version(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_APP_CRC: - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_app_checksum(i); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_SET_COLOUR: - new_cmd.led_num = OREOLED_ALL_INSTANCES; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_set_colour(i, ((oreoled_rgbset_t *) arg)->red, ((oreoled_rgbset_t *) arg)->green); - ret = OK; - } - } - - return ret; - - case OREOLED_BL_BOOT_APP: - new_cmd.led_num = OREOLED_ALL_INSTANCES; - - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - if (_healthy[i]) { - bootloader_boot(i); - ret = OK; - } - } - - return ret; - - case OREOLED_SEND_BYTES: - /* send bytes */ - new_cmd = *((oreoled_cmd_t *) arg); - - /* special handling for request to set all instances */ - if (new_cmd.led_num == OREOLED_ALL_INSTANCES) { - for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) { - /* add command to queue for all healthy leds */ - if (_healthy[i]) { - new_cmd.led_num = i; - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - } else if (new_cmd.led_num < OREOLED_NUM_LEDS) { - /* request to set individual instance's rgb value */ - if (_healthy[new_cmd.led_num]) { - _cmd_queue->force(&new_cmd); - ret = OK; - } - } - - return ret; - - case OREOLED_FORCE_SYNC: - send_general_call(); - break; - - default: - /* see if the parent class can make any use of it */ - ret = CDev::ioctl(filp, cmd, arg); - break; - } - - return ret; -} - -/* send general call on I2C bus to syncronise all LEDs */ -int -OREOLED::send_general_call() -{ - int ret = -ENODEV; - - /* set I2C address to zero */ - set_device_address(0); - - /* prepare command : 0x01 = general hardware call, 0x00 = I2C address of master (but we don't act as a slave so set to zero)*/ - uint8_t msg[] = {0x01, 0x00}; - - /* send I2C command */ - if (transfer(msg, sizeof(msg), nullptr, 0) == OK) { - ret = OK; - } - - /* record time */ - _last_gencall = hrt_absolute_time(); - - return ret; -} - -/* send a cmd to an LEDs (used for testing only) */ -int -OREOLED::send_cmd(oreoled_cmd_t new_cmd) -{ - int ret = -ENODEV; - - /* sanity check led number, health and cmd length */ - if ((new_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[new_cmd.led_num] && (new_cmd.num_bytes < OREOLED_CMD_LENGTH_MAX)) { - /* set I2C address */ - set_device_address(OREOLED_BASE_I2C_ADDR + new_cmd.led_num); - - /* add to queue */ - _cmd_queue->force(&new_cmd); - ret = OK; - } - - return ret; -} - -/* return the internal _is_ready flag indicating if initialisation is complete */ -bool -OREOLED::is_ready() -{ - return _is_ready; -} - -void -oreoled_usage() -{ - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'reset', 'rgb 30 40 50', 'macro 4', 'gencall', 'bytes 7 9 6'"); - warnx("bootloader commands: try 'blping', 'blver', 'blappver', 'blappcrc', 'blcolour ', 'blflash', 'blboot'"); - warnx("options:"); - warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); - warnx(" -a addr (0x%x)", OREOLED_BASE_I2C_ADDR); -} - -int -oreoled_main(int argc, char *argv[]) -{ - int i2cdevice = -1; - int i2c_addr = OREOLED_BASE_I2C_ADDR; /* 7bit */ - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - i2c_addr = (int)strtol(myoptarg, NULL, 0); - break; - - case 'b': - i2cdevice = (int)strtol(myoptarg, NULL, 0); - break; - - default: - oreoled_usage(); - exit(0); - } - } - - if (myoptind >= argc) { - oreoled_usage(); - exit(1); - } - - const char *verb = argv[myoptind]; - - int ret; - - /* start driver */ - if (!strcmp(verb, "start")) { - if (g_oreoled != nullptr) { - errx(1, "already started"); - } - - /* by default use LED bus */ - if (i2cdevice == -1) { - i2cdevice = PX4_I2C_BUS_LED; - } - - /* handle update flags */ - bool autoupdate = false; - bool alwaysupdate = false; - - if (argc > 2 && !strcmp(argv[2], "autoupdate")) { - warnx("autoupdate enabled"); - autoupdate = true; - - } else if (argc > 2 && !strcmp(argv[2], "alwaysupdate")) { - warnx("alwaysupdate enabled"); - alwaysupdate = true; - } - - /* instantiate driver */ - g_oreoled = new OREOLED(i2cdevice, i2c_addr, autoupdate, alwaysupdate); - - /* check if object was created */ - if (g_oreoled == nullptr) { - errx(1, "failed to allocated memory for driver"); - } - - /* check object was created successfully */ - if (g_oreoled->init() != OK) { - delete g_oreoled; - g_oreoled = nullptr; - errx(1, "failed to start driver"); - } - - /* wait for up to 20 seconds for the driver become ready */ - for (uint8_t i = 0; i < 20; i++) { - if (g_oreoled != nullptr && g_oreoled->is_ready()) { - break; - } - - sleep(1); - } - - exit(0); - } - - /* need the driver past this point */ - if (g_oreoled == nullptr) { - warnx("not started"); - oreoled_usage(); - exit(1); - } - - if (!strcmp(verb, "test")) { - int fd = open(OREOLED0_DEVICE_PATH, O_RDWR); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - /* structure to hold desired colour */ - oreoled_rgbset_t rgb_set_red = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0xFF, 0x0, 0x0}; - oreoled_rgbset_t rgb_set_blue = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, 0x0, 0x0, 0xFF}; - oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; - - /* flash red and blue for 3 seconds */ - for (uint8_t i = 0; i < 30; i++) { - /* red */ - if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_red)) != OK) { - errx(1, " failed to update rgb"); - } - - /* sleep for 0.05 seconds */ - usleep(50000); - - /* blue */ - if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_blue)) != OK) { - errx(1, " failed to update rgb"); - } - - /* sleep for 0.05 seconds */ - usleep(50000); - } - - /* turn off LED */ - if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off)) != OK) { - errx(1, " failed to turn off led"); - } - - close(fd); - exit(ret); - } - - /* display driver status */ - if (!strcmp(verb, "info")) { - g_oreoled->info(); - exit(0); - } - - if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - /* turn off LED */ - oreoled_rgbset_t rgb_set_off = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_OFF, 0x0, 0x0, 0x0}; - ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set_off); - - close(fd); - - /* delete the oreoled object if stop was requested, in addition to turning off the LED. */ - if (!strcmp(verb, "stop")) { - OREOLED *tmp_oreoled = g_oreoled; - g_oreoled = nullptr; - delete tmp_oreoled; - exit(0); - } - - exit(ret); - } - - /* send rgb request to all LEDS */ - if (!strcmp(verb, "rgb")) { - if (argc < 5) { - errx(1, "Usage: oreoled rgb "); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); - uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); - uint8_t blue = (uint8_t)strtol(argv[4], NULL, 0); - oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, blue}; - - if ((ret = ioctl(fd, OREOLED_SET_RGB, (unsigned long)&rgb_set)) != OK) { - errx(1, "failed to set rgb"); - } - - close(fd); - exit(ret); - } - - /* send macro request to all LEDS */ - if (!strcmp(verb, "macro")) { - if (argc < 3) { - errx(1, "Usage: oreoled macro "); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - uint8_t macro = (uint8_t)strtol(argv[2], NULL, 0); - - /* sanity check macro number */ - if (macro > OREOLED_PARAM_MACRO_ENUM_COUNT) { - errx(1, "invalid macro number %d", (int)macro); - exit(ret); - } - - oreoled_macrorun_t macro_run = {OREOLED_ALL_INSTANCES, (enum oreoled_macro)macro}; - - if ((ret = ioctl(fd, OREOLED_RUN_MACRO, (unsigned long)¯o_run)) != OK) { - errx(1, "failed to run macro"); - } - - close(fd); - exit(ret); - } - - /* send reset request to all LEDS */ - if (!strcmp(verb, "reset")) { - if (argc < 2) { - errx(1, "Usage: oreoled reset"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_SEND_RESET, 0)) != OK) { - errx(1, "failed to run macro"); - } - - close(fd); - exit(ret); - } - - /* attempt to flash all LEDS in bootloader mode*/ - if (!strcmp(verb, "blflash")) { - if (argc < 2) { - errx(1, "Usage: oreoled blflash"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_FLASH, 0)) != OK) { - errx(1, "failed to run flash"); - } - - close(fd); - exit(ret); - } - - /* send bootloader boot request to all LEDS */ - if (!strcmp(verb, "blboot")) { - if (argc < 2) { - errx(1, "Usage: oreoled blboot"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_BOOT_APP, 0)) != OK) { - errx(1, "failed to run boot"); - } - - close(fd); - exit(ret); - } - - /* send bootloader ping all LEDs */ - if (!strcmp(verb, "blping")) { - if (argc < 2) { - errx(1, "Usage: oreoled blping"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_PING, 0)) != OK) { - errx(1, "failed to run blping"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their bootloader version */ - if (!strcmp(verb, "blver")) { - if (argc < 2) { - errx(1, "Usage: oreoled blver"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_VER, 0)) != OK) { - errx(1, "failed to get bootloader version"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their application version */ - if (!strcmp(verb, "blappver")) { - if (argc < 2) { - errx(1, "Usage: oreoled blappver"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_APP_VER, 0)) != OK) { - errx(1, "failed to get boot app version"); - } - - close(fd); - exit(ret); - } - - /* ask all LEDs for their application crc */ - if (!strcmp(verb, "blappcrc")) { - if (argc < 2) { - errx(1, "Usage: oreoled blappcrc"); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - if ((ret = ioctl(fd, OREOLED_BL_APP_CRC, 0)) != OK) { - errx(1, "failed to get boot app crc"); - } - - close(fd); - exit(ret); - } - - /* set the default bootloader LED colour on all LEDs */ - if (!strcmp(verb, "blcolour")) { - if (argc < 4) { - errx(1, "Usage: oreoled blcolour "); - } - - int fd = open(OREOLED0_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " OREOLED0_DEVICE_PATH); - } - - uint8_t red = (uint8_t)strtol(argv[2], NULL, 0); - uint8_t green = (uint8_t)strtol(argv[3], NULL, 0); - oreoled_rgbset_t rgb_set = {OREOLED_ALL_INSTANCES, OREOLED_PATTERN_SOLID, red, green, 0}; - - if ((ret = ioctl(fd, OREOLED_BL_SET_COLOUR, (unsigned long)&rgb_set)) != OK) { - errx(1, "failed to set boot startup colours"); - } - - close(fd); - exit(ret); - } - - /* send general hardware call to all LEDS */ - if (!strcmp(verb, "gencall")) { - ret = g_oreoled->send_general_call(); - warnx("sent general call"); - exit(ret); - } - - /* send a string of bytes to an LED using send_bytes function */ - if (!strcmp(verb, "bytes")) { - if (argc < 3) { - errx(1, "Usage: oreoled bytes ..."); - } - - /* structure to be sent */ - oreoled_cmd_t sendb; - - /* maximum of 20 bytes can be sent */ - if (argc > 20 + 3) { - errx(1, "Max of 20 bytes can be sent"); - } - - /* check led num */ - sendb.led_num = (uint8_t)strtol(argv[myoptind + 1], NULL, 0); - - if (sendb.led_num > 3) { - errx(1, "led number must be between 0 ~ 3"); - } - - /* get bytes */ - sendb.num_bytes = argc - (myoptind + 2); - uint8_t byte_count; - - for (byte_count = 0; byte_count < sendb.num_bytes; byte_count++) { - sendb.buff[byte_count] = (uint8_t)strtol(argv[byte_count + myoptind + 2], NULL, 0); - } - - /* send bytes */ - if ((ret = g_oreoled->send_cmd(sendb)) != OK) { - errx(1, "failed to send command"); - - } else { - warnx("sent %d bytes", (int)sendb.num_bytes); - } - - exit(ret); - } - - oreoled_usage(); - exit(0); -} diff --git a/src/drivers/lights/pca8574/pca8574.cpp b/src/drivers/lights/pca8574/pca8574.cpp deleted file mode 100644 index 944cb6de45e3..000000000000 --- a/src/drivers/lights/pca8574/pca8574.cpp +++ /dev/null @@ -1,520 +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 pca8574.cpp - * - * Driver for an 8 I/O controller (PC8574) connected via I2C. - * - * @author Lorenz Meier - * @author Julian Oes - * @author Anton Babushkin - */ - -#include - -#include -#include -#include -#include -#include - - -#define PCA8574_ONTIME 120 -#define PCA8574_OFFTIME 120 -#define PCA8574_DEVICE_PATH "/dev/pca8574" - -#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) - -class PCA8574 : public device::I2C, public px4::ScheduledWorkItem -{ -public: - PCA8574(int bus, int pca8574); - virtual ~PCA8574() = default; - - virtual int probe(); - virtual int info(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - bool is_running() { return _running; } - -private: - uint8_t _values_out; - uint8_t _values_in; - uint8_t _blinking; - uint8_t _blink_phase; - - enum IOX_MODE _mode; - bool _running; - int _led_interval; - bool _should_run; - bool _update_out; - int _counter; - - void Run() override; - - int send_led_enable(uint8_t arg); - int send_led_values(); - - int get(uint8_t &vals); -}; - -/* for now, we only support one PCA8574 */ -namespace -{ -PCA8574 *g_pca8574; -} - -void pca8574_usage(); - -extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); - -PCA8574::PCA8574(int bus, int pca8574) : - I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _values_out(0), - _values_in(0), - _blinking(0), - _blink_phase(0), - _mode(IOX_MODE_OFF), - _running(false), - _led_interval(80), - _should_run(false), - _update_out(false), - _counter(0) -{ -} - -int -PCA8574::probe() -{ - uint8_t val; - return get(val); -} - -int -PCA8574::info() -{ - int ret = OK; - - return ret; -} - -int -PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - int ret = ENOTTY; - - switch (cmd) { - case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { - // set the specified on / off state - uint8_t position = (1 << (cmd - IOX_SET_VALUE)); - uint8_t prev = _values_out; - - if (arg) { - _values_out |= position; - - } else { - _values_out &= ~(position); - } - - if (_values_out != prev) { - if (_values_out) { - _mode = IOX_MODE_ON; - } - - send_led_values(); - } - - return OK; - } - - case IOX_SET_MASK: - send_led_enable(arg); - return OK; - - case IOX_GET_MASK: { - uint8_t val; - ret = get(val); - - if (ret == OK) { - return val; - - } else { - return -1; - } - } - - case IOX_SET_MODE: - - if (_mode != (IOX_MODE)arg) { - - switch ((IOX_MODE)arg) { - case IOX_MODE_OFF: - _values_out = 0xFF; - break; - - case IOX_MODE_ON: - _values_out = 0; - break; - - case IOX_MODE_TEST_OUT: - break; - - default: - return -1; - } - - _mode = (IOX_MODE)arg; - send_led_values(); - } - - return OK; - - default: - // see if the parent class can make any use of it - ret = CDev::ioctl(filp, cmd, arg); - break; - } - - return ret; -} - -/** - * Main loop function - */ -void -PCA8574::Run() -{ - if (_mode == IOX_MODE_TEST_OUT) { - - // we count only seven states - _counter &= 0xF; - _counter++; - - for (int i = 0; i < 8; i++) { - if (i < _counter) { - _values_out |= (1 << i); - - } else { - _values_out &= ~(1 << i); - } - } - - _update_out = true; - _should_run = true; - - } else if (_mode == IOX_MODE_OFF) { - _update_out = true; - _should_run = false; - - } else { - - // Any of the normal modes - if (_blinking > 0) { - /* we need to be running to blink */ - _should_run = true; - - } else { - _should_run = false; - } - } - - if (_update_out) { - uint8_t msg; - - if (_blinking) { - msg = (_values_out & _blinking & _blink_phase); - - // wipe out all positions that are marked as blinking - msg &= ~(_blinking); - - // fill blink positions - msg |= ((_blink_phase) ? _blinking : 0); - - _blink_phase = !_blink_phase; - - } else { - msg = _values_out; - } - - int ret = transfer(&msg, sizeof(msg), nullptr, 0); - - if (!ret) { - _update_out = false; - } - } - - // check if any activity remains, else stp - if (!_should_run) { - _running = false; - return; - } - - // re-queue ourselves to run again later - _running = true; - - ScheduleDelayed(_led_interval); -} - -/** - * Sent ENABLE flag to LED driver - */ -int -PCA8574::send_led_enable(uint8_t arg) -{ - int ret = transfer(&arg, sizeof(arg), nullptr, 0); - - return ret; -} - -/** - * Send 8 outputs - */ -int -PCA8574::send_led_values() -{ - _update_out = true; - - // if not active, kick it - if (!_running) { - _running = true; - ScheduleNow(); - } - - return 0; -} - -int -PCA8574::get(uint8_t &vals) -{ - uint8_t result; - int ret; - - ret = transfer(nullptr, 0, &result, 1); - - if (ret == OK) { - _values_in = result; - vals = result; - } - - return ret; -} - -void -pca8574_usage() -{ - warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); - warnx("options:"); - warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); - warnx(" -a addr (0x%x)", ADDR); -} - -int -pca8574_main(int argc, char *argv[]) -{ - int i2cdevice = -1; - int pca8574adr = ADDR; // 7bit - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - - // jump over start/off/etc and look at options first - while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'a': - pca8574adr = strtol(myoptarg, NULL, 0); - break; - - case 'b': - i2cdevice = strtol(myoptarg, NULL, 0); - break; - - default: - pca8574_usage(); - exit(0); - } - } - - if (myoptind >= argc) { - pca8574_usage(); - exit(0); - } - - const char *verb = argv[myoptind]; - - int fd; - int ret; - - if (!strcmp(verb, "start")) { - if (g_pca8574 != nullptr) { - errx(1, "already started"); - } - - if (i2cdevice == -1) { - // try the external bus first - i2cdevice = PX4_I2C_BUS_EXPANSION; - g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); - - if (g_pca8574 != nullptr && OK != g_pca8574->init()) { - delete g_pca8574; - g_pca8574 = nullptr; - } - - if (g_pca8574 == nullptr) { - // fall back to default bus - if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { - errx(1, "init failed"); - } - - i2cdevice = PX4_I2C_BUS_LED; - } - } - - if (g_pca8574 == nullptr) { - g_pca8574 = new PCA8574(i2cdevice, pca8574adr); - - if (g_pca8574 == nullptr) { - errx(1, "new failed"); - } - - if (OK != g_pca8574->init()) { - delete g_pca8574; - g_pca8574 = nullptr; - errx(1, "init failed"); - } - } - - exit(0); - } - - // need the driver past this point - if (g_pca8574 == nullptr) { - warnx("not started, run pca8574 start"); - exit(1); - } - - if (!strcmp(verb, "test")) { - fd = open(PCA8574_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA8574_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); - - close(fd); - exit(ret); - } - - if (!strcmp(verb, "info")) { - g_pca8574->info(); - exit(0); - } - - if (!strcmp(verb, "off")) { - fd = open(PCA8574_DEVICE_PATH, 0); - - if (fd < 0) { - errx(1, "Unable to open " PCA8574_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); - close(fd); - exit(ret); - } - - if (!strcmp(verb, "stop")) { - fd = open(PCA8574_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA8574_DEVICE_PATH); - } - - ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); - close(fd); - - // wait until we're not running any more - for (unsigned i = 0; i < 15; i++) { - if (!g_pca8574->is_running()) { - break; - } - - usleep(50000); - printf("."); - fflush(stdout); - } - - printf("\n"); - fflush(stdout); - - if (!g_pca8574->is_running()) { - delete g_pca8574; - g_pca8574 = nullptr; - exit(0); - - } else { - warnx("stop failed."); - exit(1); - } - } - - if (!strcmp(verb, "val")) { - if (argc < 4) { - errx(1, "Usage: pca8574 val <0 or 1>"); - } - - fd = open(PCA8574_DEVICE_PATH, 0); - - if (fd == -1) { - errx(1, "Unable to open " PCA8574_DEVICE_PATH); - } - - unsigned channel = strtol(argv[2], NULL, 0); - unsigned val = strtol(argv[3], NULL, 0); - - if (channel < 8) { - ret = ioctl(fd, (IOX_SET_VALUE + channel), val); - - } else { - ret = -1; - } - - close(fd); - exit(ret); - } - - pca8574_usage(); - exit(0); -} diff --git a/src/drivers/lights/rgbled/CMakeLists.txt b/src/drivers/lights/rgbled/CMakeLists.txt index 304c0b468626..fd69c3e8363f 100644 --- a/src/drivers/lights/rgbled/CMakeLists.txt +++ b/src/drivers/lights/rgbled/CMakeLists.txt @@ -34,8 +34,6 @@ px4_add_module( MODULE drivers__rgbled MAIN rgbled - STACK_MAIN - 1500 SRCS rgbled.cpp DEPENDS diff --git a/src/drivers/lights/rgbled/rgbled.cpp b/src/drivers/lights/rgbled/rgbled.cpp index f5b8e9a663be..a15a584326f9 100644 --- a/src/drivers/lights/rgbled/rgbled.cpp +++ b/src/drivers/lights/rgbled/rgbled.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * 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 @@ -45,11 +45,9 @@ #include #include #include -#include -#include "uORB/topics/parameter_update.h" - -#define RGBLED_ONTIME 120 -#define RGBLED_OFFTIME 120 +#include +#include +#include #define ADDR 0x55 /**< I2C adress of TCA62724FMG */ #define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */ @@ -61,29 +59,28 @@ #define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */ #define SETTING_ENABLE 0x02 /**< on */ - class RGBLED : public device::I2C, public px4::ScheduledWorkItem { public: RGBLED(int bus, int rgbled); virtual ~RGBLED(); - virtual int init(); virtual int probe(); - int status(); + int status(); private: - float _brightness; - float _max_brightness; - uint8_t _r; - uint8_t _g; - uint8_t _b; - volatile bool _running; - volatile bool _should_run; - bool _leds_enabled; - int _param_sub; + float _brightness{1.0f}; + float _max_brightness{1.0f}; + + 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::Subscription _param_sub{ORB_ID(parameter_update)}; LedController _led_controller; @@ -92,7 +89,7 @@ class RGBLED : public device::I2C, public px4::ScheduledWorkItem 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(); + void update_params(); }; /* for now, we only support one RGBLED */ @@ -107,16 +104,7 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _brightness(1.0f), - _max_brightness(1.0f), - _r(0), - _g(0), - _b(0), - _running(false), - _should_run(true), - _leds_enabled(true), - _param_sub(-1) + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { } @@ -133,8 +121,7 @@ RGBLED::~RGBLED() int RGBLED::init() { - int ret; - ret = I2C::init(); + int ret = I2C::init(); if (ret != OK) { return ret; @@ -187,11 +174,10 @@ RGBLED::probe() int RGBLED::status() { - int ret; bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + int ret = get(on, powersave, r, g, b); if (ret == OK) { /* we don't care about power-save mode */ @@ -212,40 +198,19 @@ void RGBLED::Run() { if (!_should_run) { - if (_param_sub >= 0) { - orb_unsubscribe(_param_sub); - } - - int led_control_sub = _led_controller.led_control_subscription(); - - if (led_control_sub >= 0) { - orb_unsubscribe(led_control_sub); - } - _running = false; return; } - if (_param_sub < 0) { - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } + if (_param_sub.updated()) { + // clear update + parameter_update_s pupdate; + _param_sub.copy(&pupdate); - if (_param_sub >= 0) { - bool updated = false; - orb_check(_param_sub, &updated); + update_params(); - if (updated) { - parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - update_params(); - // Immediately update to change brightness - send_led_rgb(); - } + // Immediately update to change brightness + send_led_rgb(); } LedControlData led_control_data; diff --git a/src/drivers/lights/rgbled_ncp5623c/CMakeLists.txt b/src/drivers/lights/rgbled_ncp5623c/CMakeLists.txt index b44b386842a8..951e1aa1d554 100755 --- a/src/drivers/lights/rgbled_ncp5623c/CMakeLists.txt +++ b/src/drivers/lights/rgbled_ncp5623c/CMakeLists.txt @@ -34,8 +34,6 @@ px4_add_module( MODULE drivers__rgbled_ncp5623c MAIN rgbled_ncp5623c - STACK_MAIN - 1500 SRCS rgbled_ncp5623c.cpp DEPENDS diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index 2a994348ca42..a794902ed532 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,8 +44,9 @@ #include #include #include -#include -#include "uORB/topics/parameter_update.h" +#include +#include +#include #define ADDR 0x39 /**< I2C adress of NCP5623C */ @@ -65,21 +66,21 @@ class RGBLED_NPC5623C : public device::I2C, public px4::ScheduledWorkItem RGBLED_NPC5623C(int bus, int rgbled); virtual ~RGBLED_NPC5623C(); - virtual int init(); virtual int probe(); + private: - float _brightness; - float _max_brightness; + float _brightness{1.0f}; + float _max_brightness{1.0f}; - uint8_t _r; - uint8_t _g; - uint8_t _b; - volatile bool _running; - volatile bool _should_run; - bool _leds_enabled; - int _param_sub; + 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::Subscription _param_sub{ORB_ID(parameter_update)}; LedController _led_controller; @@ -103,16 +104,7 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]); RGBLED_NPC5623C::RGBLED_NPC5623C(int bus, int rgbled) : I2C("rgbled1", RGBLED1_DEVICE_PATH, bus, rgbled, 100000), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _brightness(1.0f), - _max_brightness(1.0f), - _r(0), - _g(0), - _b(0), - _running(false), - _should_run(true), - _leds_enabled(true), - _param_sub(-1) + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())) { } @@ -122,7 +114,7 @@ RGBLED_NPC5623C::~RGBLED_NPC5623C() int counter = 0; while (_running && ++counter < 10) { - usleep(100000); + px4_usleep(100000); } } @@ -140,8 +132,7 @@ RGBLED_NPC5623C::write(uint8_t reg, uint8_t data) int RGBLED_NPC5623C::init() { - int ret; - ret = I2C::init(); + int ret = I2C::init(); if (ret != OK) { return ret; @@ -171,40 +162,19 @@ void RGBLED_NPC5623C::Run() { if (!_should_run) { - if (_param_sub >= 0) { - orb_unsubscribe(_param_sub); - } - - int led_control_sub = _led_controller.led_control_subscription(); - - if (led_control_sub >= 0) { - orb_unsubscribe(led_control_sub); - } - _running = false; return; } - if (_param_sub < 0) { - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } + if (_param_sub.updated()) { + // clear update + parameter_update_s pupdate; + _param_sub.copy(&pupdate); - if (_param_sub >= 0) { - bool updated = false; - orb_check(_param_sub, &updated); + update_params(); - if (updated) { - parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - update_params(); - // Immediately update to change brightness - send_led_rgb(); - } + // Immediately update to change brightness + send_led_rgb(); } LedControlData led_control_data; diff --git a/src/drivers/lights/rgbled_pwm/CMakeLists.txt b/src/drivers/lights/rgbled_pwm/CMakeLists.txt index dc85022c7932..2001cdc4921d 100644 --- a/src/drivers/lights/rgbled_pwm/CMakeLists.txt +++ b/src/drivers/lights/rgbled_pwm/CMakeLists.txt @@ -34,10 +34,10 @@ px4_add_module( MODULE drivers__rgbled_pwm MAIN rgbled_pwm - STACK_MAIN - 1500 SRCS rgbled_pwm.cpp DEPENDS led - ) \ No newline at end of file + arch_io_pins + arch_led_pwm + ) diff --git a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp index c8bb872aa864..f49e569d58c7 100644 --- a/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp +++ b/src/drivers/lights/rgbled_pwm/rgbled_pwm.cpp @@ -44,11 +44,7 @@ #include #include #include -#include -#include - -#define RGBLED_ONTIME 120 -#define RGBLED_OFFTIME 120 +#include class RGBLED_PWM : public device::CDev, public px4::ScheduledWorkItem { @@ -56,19 +52,18 @@ class RGBLED_PWM : public device::CDev, public px4::ScheduledWorkItem RGBLED_PWM(); virtual ~RGBLED_PWM(); - virtual int init(); virtual int probe(); - int status(); + int status(); private: - uint8_t _r; - uint8_t _g; - uint8_t _b; + uint8_t _r{0}; + uint8_t _g{0}; + uint8_t _b{0}; - volatile bool _running; - volatile bool _should_run; + volatile bool _running{false}; + volatile bool _should_run{true}; LedController _led_controller; @@ -93,12 +88,7 @@ RGBLED_PWM *g_rgbled = nullptr; RGBLED_PWM::RGBLED_PWM() : CDev("rgbled_pwm", RGBLED_PWM0_DEVICE_PATH), - ScheduledWorkItem(px4::wq_configurations::lp_default), - _r(0), - _g(0), - _b(0), - _running(false), - _should_run(true) + ScheduledWorkItem(px4::wq_configurations::lp_default) { } @@ -108,7 +98,7 @@ RGBLED_PWM::~RGBLED_PWM() int counter = 0; while (_running && ++counter < 10) { - usleep(100000); + px4_usleep(100000); } } @@ -128,14 +118,19 @@ RGBLED_PWM::init() return OK; } +int +RGBLED_PWM::probe() +{ + return PX4_OK; +} + int RGBLED_PWM::status() { - int ret; bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + int ret = get(on, powersave, r, g, b); if (ret == OK) { /* we don't care about power-save mode */ @@ -148,11 +143,6 @@ RGBLED_PWM::status() return ret; } -int -RGBLED_PWM::probe() -{ - return (OK); -} /** * Main loop function @@ -161,21 +151,10 @@ void RGBLED_PWM::Run() { if (!_should_run) { - int led_control_sub = _led_controller.led_control_subscription(); - - if (led_control_sub >= 0) { - orb_unsubscribe(led_control_sub); - } - _running = false; return; } - if (!_led_controller.is_init()) { - int led_control_sub = orb_subscribe(ORB_ID(led_control)); - _led_controller.init(led_control_sub); - } - LedControlData led_control_data; if (_led_controller.update(led_control_data) == 1) { @@ -280,58 +259,60 @@ rgbled_pwm_main(int argc, char *argv[]) default: rgbled_usage(); - exit(0); + return 1; } } if (myoptind >= argc) { rgbled_usage(); - exit(0); + return 1; } const char *verb = argv[myoptind]; - if (!strcmp(verb, "start")) { if (g_rgbled != nullptr) { - errx(1, "already started"); + PX4_WARN("already started"); + return 1; } if (g_rgbled == nullptr) { g_rgbled = new RGBLED_PWM(); if (g_rgbled == nullptr) { - errx(1, "new failed"); + PX4_WARN("alloc failed"); + return 1; } if (OK != g_rgbled->init()) { delete g_rgbled; g_rgbled = nullptr; - errx(1, "init failed"); + PX4_ERR("init failed"); + return 1; } } - exit(0); + return 0; } /* need the driver past this point */ if (g_rgbled == nullptr) { PX4_WARN("not started"); rgbled_usage(); - exit(1); + return 1; } if (!strcmp(verb, "status")) { g_rgbled->status(); - exit(0); + return 0; } if (!strcmp(verb, "stop")) { delete g_rgbled; g_rgbled = nullptr; - exit(0); + return 0; } rgbled_usage(); - exit(0); + return 1; } diff --git a/src/drivers/linux_pwm_out/CMakeLists.txt b/src/drivers/linux_pwm_out/CMakeLists.txt index 6b3779884836..5d19e2ffc309 100644 --- a/src/drivers/linux_pwm_out/CMakeLists.txt +++ b/src/drivers/linux_pwm_out/CMakeLists.txt @@ -42,6 +42,6 @@ px4_add_module( ocpoc_mmap.cpp bbblue_pwm_rc.cpp DEPENDS - pwm_limit + output_limit ) diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index c8112dd9a0a3..2e6db9ce33e5 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -54,7 +54,7 @@ #include #include #include -#include +#include #include #include "common.h" @@ -98,7 +98,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uint32_t _groups_required = 0; uint32_t _groups_subscribed = 0; -pwm_limit_t _pwm_limit; +output_limit_t _pwm_limit; // esc parameters int32_t _pwm_disarmed; @@ -268,7 +268,7 @@ void task_main(int argc, char *argv[]) _armed.armed = false; _armed.prearmed = false; - pwm_limit_init(&_pwm_limit); + output_limit_init(&_pwm_limit); while (!_task_should_exit) { @@ -332,7 +332,7 @@ void task_main(int argc, char *argv[]) } /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; + _pwm_limit.state = OUTPUT_LIMIT_STATE_ON; } if (_mixer_group != nullptr) { @@ -358,16 +358,16 @@ void task_main(int argc, char *argv[]) uint16_t pwm[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS]; // TODO FIXME: pre-armed seems broken - pwm_limit_calc(_armed.armed, - false/*_armed.prearmed*/, - _outputs.noutputs, - reverse_mask, - disarmed_pwm, - min_pwm, - max_pwm, - _outputs.output, - pwm, - &_pwm_limit); + output_limit_calc(_armed.armed, + false/*_armed.prearmed*/, + _outputs.noutputs, + reverse_mask, + disarmed_pwm, + min_pwm, + max_pwm, + _outputs.output, + pwm, + &_pwm_limit); if (_armed.lockdown || _armed.manual_lockdown) { pwm_out->send_output_pwm(disarmed_pwm, _outputs.noutputs); diff --git a/src/drivers/linux_sbus/linux_sbus.h b/src/drivers/linux_sbus/linux_sbus.h index be51d1db0828..e03a849fd417 100644 --- a/src/drivers/linux_sbus/linux_sbus.h +++ b/src/drivers/linux_sbus/linux_sbus.h @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/magnetometer/ak09916/CMakeLists.txt b/src/drivers/magnetometer/ak09916/CMakeLists.txt index 6bedb1c54eed..4bf7b8708501 100644 --- a/src/drivers/magnetometer/ak09916/CMakeLists.txt +++ b/src/drivers/magnetometer/ak09916/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__ak09916 MAIN ak09916 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp index 5e6f15dc657b..d42ff990985f 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/ak09916/ak09916.hpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #define AK09916_DEVICE_PATH_MAG "/dev/ak09916_i2c_int" diff --git a/src/drivers/magnetometer/bmm150/CMakeLists.txt b/src/drivers/magnetometer/bmm150/CMakeLists.txt index 6adda9d085c6..0622ba76e840 100644 --- a/src/drivers/magnetometer/bmm150/CMakeLists.txt +++ b/src/drivers/magnetometer/bmm150/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__bmm150 MAIN bmm150 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/bmm150/bmm150.hpp b/src/drivers/magnetometer/bmm150/bmm150.hpp index 78411ae804a1..f9e1fe3d5877 100644 --- a/src/drivers/magnetometer/bmm150/bmm150.hpp +++ b/src/drivers/magnetometer/bmm150/bmm150.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/magnetometer/hmc5883/CMakeLists.txt b/src/drivers/magnetometer/hmc5883/CMakeLists.txt index 6186e6b4b0ff..9933945fe00f 100644 --- a/src/drivers/magnetometer/hmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/hmc5883/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__hmc5883 MAIN hmc5883 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/hmc5883/hmc5883.cpp b/src/drivers/magnetometer/hmc5883/hmc5883.cpp index f4ef237427ef..1a6e51211e39 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883.cpp @@ -57,7 +57,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/magnetometer/ist8310/CMakeLists.txt b/src/drivers/magnetometer/ist8310/CMakeLists.txt index 553e18d76541..a2bfd2305721 100644 --- a/src/drivers/magnetometer/ist8310/CMakeLists.txt +++ b/src/drivers/magnetometer/ist8310/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MAIN ist8310 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable - STACK_MAIN 1500 SRCS ist8310.cpp DEPENDS diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index ddad228ea322..852a59437f4a 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -76,7 +76,7 @@ #include #include -#include +#include /* * IST8310 internal constants and data structures. diff --git a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt index 2763852bde33..5d159fbbaf7a 100644 --- a/src/drivers/magnetometer/lis3mdl/CMakeLists.txt +++ b/src/drivers/magnetometer/lis3mdl/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__lis3mdl MAIN lis3mdl - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl.h b/src/drivers/magnetometer/lis3mdl/lis3mdl.h index c00ebb9943a2..a77e3e85ad45 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index fcf96ce2aac9..6342c098ad94 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include // Register mapping diff --git a/src/drivers/magnetometer/qmc5883/CMakeLists.txt b/src/drivers/magnetometer/qmc5883/CMakeLists.txt index e674ba62758a..4a2b6f465076 100644 --- a/src/drivers/magnetometer/qmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/qmc5883/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__qmc5883 MAIN qmc5883 - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/qmc5883/qmc5883.cpp b/src/drivers/magnetometer/qmc5883/qmc5883.cpp index 42864a3d2acc..6e3665e87e24 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883.cpp +++ b/src/drivers/magnetometer/qmc5883/qmc5883.cpp @@ -56,7 +56,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/magnetometer/rm3100/CMakeLists.txt b/src/drivers/magnetometer/rm3100/CMakeLists.txt index 7d9248c47cd3..f464153c495a 100644 --- a/src/drivers/magnetometer/rm3100/CMakeLists.txt +++ b/src/drivers/magnetometer/rm3100/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__rm3100 MAIN rm3100 - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index 1d7ffc24adda..83492e143336 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -52,7 +52,7 @@ #include #include -#include +#include /** * RM3100 internal constants and data structures. diff --git a/src/drivers/optical_flow/CMakeLists.txt b/src/drivers/optical_flow/CMakeLists.txt index 6040da074524..63a6932056a6 100644 --- a/src/drivers/optical_flow/CMakeLists.txt +++ b/src/drivers/optical_flow/CMakeLists.txt @@ -31,5 +31,6 @@ # ############################################################################ -add_subdirectory(px4flow) +add_subdirectory(paw3902) add_subdirectory(pmw3901) +add_subdirectory(px4flow) diff --git a/src/drivers/lights/pca8574/CMakeLists.txt b/src/drivers/optical_flow/paw3902/CMakeLists.txt similarity index 91% rename from src/drivers/lights/pca8574/CMakeLists.txt rename to src/drivers/optical_flow/paw3902/CMakeLists.txt index de66d2ef643d..1683937e5cfe 100644 --- a/src/drivers/lights/pca8574/CMakeLists.txt +++ b/src/drivers/optical_flow/paw3902/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,11 +30,11 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__pca8574 - MAIN pca8574 - COMPILE_FLAGS + MODULE drivers__optical_flow__paw3902 + MAIN paw3902 SRCS - pca8574.cpp - DEPENDS + paw3902_main.cpp + PAW3902.cpp ) diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp new file mode 100644 index 000000000000..73edfca294a1 --- /dev/null +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -0,0 +1,705 @@ +/**************************************************************************** + * + * 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 "PAW3902.hpp" + +PAW3902::PAW3902(int bus, enum Rotation yaw_rotation) : + SPI("PAW3902", nullptr, bus, PAW3902_SPIDEV, SPIDEV_MODE0, PAW3902_SPI_BUS_SPEED), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _sample_perf(perf_alloc(PC_ELAPSED, "paw3902: read")), + _interval_perf(perf_alloc(PC_INTERVAL, "paw3902: interval")), + _comms_errors(perf_alloc(PC_COUNT, "paw3902: com_err")), + _dupe_count_perf(perf_alloc(PC_COUNT, "paw3902: duplicate reading")), + _yaw_rotation(yaw_rotation) +{ +} + +PAW3902::~PAW3902() +{ + // make sure we are truly inactive + stop(); + + // free perf counters + perf_free(_sample_perf); + perf_free(_interval_perf); + perf_free(_comms_errors); + perf_free(_dupe_count_perf); +} + + +int +PAW3902::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; + + if (param_get(rot, &val) == PX4_OK) { + _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); + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + return PX4_ERROR; + } + + reset(); + + // default to low light mode (1) + modeLowLight(); + + _previous_collect_timestamp = hrt_absolute_time(); + + start(); + + return PX4_OK; +} + +int +PAW3902::probe() +{ + uint8_t product_id = registerRead(Register::Product_ID); + + PX4_INFO("DEVICE_ID: %X", product_id); + + // Test the SPI communication, checking chipId and inverse chipId + if (product_id != PRODUCT_ID) { + return PX4_ERROR; + } + + uint8_t revision_id = registerRead(Register::Revision_ID); + PX4_INFO("REVISION_ID: %X", revision_id); + + if (revision_id != REVISION_ID) { + return PX4_ERROR; + } + + return PX4_OK; +} + +bool +PAW3902::reset() +{ + // Power on reset + registerWrite(Register::Power_Up_Reset, 0x5A); + usleep(5000); + + // 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); + + usleep(1000); + + return true; +} + +bool +PAW3902::changeMode(Mode newMode) +{ + if (newMode != _mode) { + PX4_INFO("changing from mode %d -> %d", static_cast(_mode), static_cast(newMode)); + ScheduleClear(); + reset(); + + switch (newMode) { + case Mode::Bright: + modeBright(); + ScheduleOnInterval(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + modeLowLight(); + ScheduleOnInterval(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + modeSuperLowLight(); + ScheduleOnInterval(SAMPLE_INTERVAL_MODE_2); + break; + } + + _mode = newMode; + } + + // 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_INFO("Resolution: %X", resolution); + PX4_INFO("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f))); + + return true; +} + +bool +PAW3902::modeBright() +{ + // Mode 0: Bright (126 fps) 60 Lux typical + + // set performance optimization registers + 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); + + usleep(10_ms); // 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); + + usleep(10_ms); // delay 10ms + + registerWrite(0x73, 0x00); + + return false; +} + +bool +PAW3902::modeLowLight() +{ + // Mode 1: Low Light (126 fps) 30 Lux typical + // low light and low speed motion tracking + + // set performance optimization registers + 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); + + usleep(10_ms); // 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); + + usleep(10_ms); // delay 10ms + + registerWrite(0x73, 0x00); + + return false; +} + +bool +PAW3902::modeSuperLowLight() +{ + // Mode 2: Super Low Light (50 fps) 9 Lux typical + // super low light and low speed motion tracking + + // set performance optimization registers + 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); + + usleep(25_ms); // 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); + + usleep(25_ms); // delay 25ms + + registerWrite(0x73, 0x00); + + return true; +} + +uint8_t +PAW3902::registerRead(uint8_t reg) +{ + uint8_t cmd[2] {}; + cmd[0] = reg; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + up_udelay(T_SRR); + return cmd[1]; +} + +void +PAW3902::registerWrite(uint8_t reg, uint8_t data) +{ + uint8_t cmd[2]; + cmd[0] = DIR_WRITE(reg); + cmd[1] = data; + transfer(&cmd[0], nullptr, sizeof(cmd)); + up_udelay(T_SWW); +} + +void +PAW3902::Run() +{ + perf_begin(_sample_perf); + perf_count(_interval_perf); + + struct TransferBuffer { + uint8_t cmd = Register::Motion_Burst; + BURST_TRANSFER data{}; + }; + TransferBuffer 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); + return; + } + + const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp; + _flow_dt_sum_usec += dt_flow; + _frame_count_since_last++; + + // update for next iteration + _previous_collect_timestamp = timestamp_sample; + + // PX4_INFO("data.Motion %d", buf.data.Motion); + // PX4_INFO("data.Observation %d", buf.data.Observation); + // PX4_INFO("data.Delta_X_L %d", buf.data.Delta_X_L); + // PX4_INFO("data.Delta_X_H %d", buf.data.Delta_X_H); + // PX4_INFO("data.Delta_Y_L %d", buf.data.Delta_Y_L); + // PX4_INFO("data.Delta_Y_H %d", buf.data.Delta_Y_H); + // PX4_INFO("data.SQUAL %d", buf.data.SQUAL); + // PX4_INFO("data.RawData_Sum %d", buf.data.RawData_Sum); + // PX4_INFO("data.Maximum_RawData %d", buf.data.Maximum_RawData); + // PX4_INFO("data.Minimum_RawData %d", buf.data.Minimum_RawData); + // PX4_INFO("data.Shutter_Upper %d", buf.data.Shutter_Upper); + // PX4_INFO("data.Shutter_Lower %d", buf.data.Shutter_Lower); + + const int16_t delta_x_raw = ((int16_t)buf.data.Delta_X_H << 8) | buf.data.Delta_X_L; + const int16_t delta_y_raw = ((int16_t)buf.data.Delta_Y_H << 8) | buf.data.Delta_Y_L; + + // 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; + + if ((buf.data.SQUAL < 0x19) && (shutter >= 0x0BC0)) { + PX4_ERR("false motion report, discarding"); + perf_end(_sample_perf); + return; + } + + switch (_mode) { + case Mode::Bright: + if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) { // AND valid for 10 consecutive frames? + // Bright -> LowLight + changeMode(Mode::LowLight); + } + + break; + + case Mode::LowLight: + if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) { // AND valid for 10 consecutive frames? + // LowLight -> SuperLowLight + changeMode(Mode::SuperLowLight); + + } else if ((shutter >= 0x0BB8)) { // AND valid for 10 consecutive frames? + // LowLight -> Bright + changeMode(Mode::Bright); + } + + break; + + case Mode::SuperLowLight: + + // SuperLowLight -> LowLight + if ((shutter >= 0x03E8)) { // AND valid for 10 consecutive frames? + changeMode(Mode::LowLight); + } + + // PAW3902JF should not operate with Shutter < 0x01F4 in Mode 2 + if (shutter >= 0x01F4) { + changeMode(Mode::LowLight); + } + + break; + } + + // TODO: page 35 switching scheme + + // As a minimum requirement, PAW3902JF should not operate with Shutter < 0x01F4 in Mode 2, and must switch to the next operation mode. + + _flow_sum_x += delta_x_raw; + _flow_sum_y += delta_y_raw; + + // returns if the collect time has not been reached + if (_flow_dt_sum_usec < _collect_time) { + perf_end(_sample_perf); + return; + } + + optical_flow_s report{}; + report.timestamp = timestamp_sample; + + + //PX4_INFO("X: %d Y: %d", _flow_sum_x, _flow_sum_y); + + report.pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians + report.pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians + + // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + float zeroval = 0.0f; + rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); + + report.frame_count_since_last_readout = _frame_count_since_last; + report.integration_timespan = _flow_dt_sum_usec; // microseconds + + report.sensor_id = 0; + report.quality = buf.data.SQUAL; + + /* No gyro on this board */ + report.gyro_x_rate_integral = NAN; + report.gyro_y_rate_integral = NAN; + report.gyro_z_rate_integral = NAN; + + // set (conservative) specs according to datasheet + report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = 30.0f; // Datasheet: infinity + + _optical_flow_pub.publish(report); + + // reset + _flow_dt_sum_usec = 0; + _flow_sum_x = 0; + _flow_sum_y = 0; + _frame_count_since_last = 0; + + perf_end(_sample_perf); +} + +void +PAW3902::start() +{ + // schedule a cycle to start things + ScheduleOnInterval(SAMPLE_INTERVAL_MODE_1); +} + +void +PAW3902::stop() +{ + ScheduleClear(); +} + +void +PAW3902::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_dupe_count_perf); +} diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp new file mode 100644 index 000000000000..8dd56eb11b9c --- /dev/null +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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 paw3902.cpp + * + * Driver for the Pixart PAW3902 optical flow sensor connected via SPI. + */ + +#pragma once + +#include "PixArt_PAW3902JF_Registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ + +#if defined PX4_SPI_BUS_EXPANSION // crazyflie +# define PAW3902_BUS PX4_SPI_BUS_EXPANSION +#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5 +# define PAW3902_BUS PX4_SPI_BUS_EXTERNAL1 +#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi +# define PAW3902_BUS PX4_SPI_BUS_EXTERNAL +#else +# error "add the required spi bus from board_config.h here" +#endif + +#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck +# define PAW3902_SPIDEV PX4_SPIDEV_EXPANSION_2 +#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1 +# define PAW3902_SPIDEV PX4_SPIDEV_EXTERNAL1_1 +#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi +# define PAW3902_SPIDEV PX4_SPIDEV_EXTERNAL +#else +# error "add the required spi dev from board_config.h here" +#endif + +#define PAW3902_SPI_BUS_SPEED (2000000L) // 2MHz + +#define DIR_WRITE(a) ((a) | (1 << 7)) +#define DIR_READ(a) ((a) & 0x7f) + +using namespace time_literals; +using namespace PixArt_PAW3902JF; + +// PAW3902JF-TXQT is PixArt Imaging + +class PAW3902 : public device::SPI, public px4::ScheduledWorkItem +{ +public: + PAW3902(int bus = PAW3902_BUS, enum Rotation yaw_rotation = ROTATION_NONE); + virtual ~PAW3902(); + + virtual int init(); + + void print_info(); + + void start(); + void stop(); + +protected: + virtual int probe(); + +private: + + void Run() override; + + uint8_t registerRead(uint8_t reg); + void registerWrite(uint8_t reg, uint8_t data); + + bool reset(); + + bool modeBright(); + bool modeLowLight(); + bool modeSuperLowLight(); + + bool changeMode(Mode newMode); + + uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + + perf_counter_t _sample_perf; + perf_counter_t _interval_perf; + perf_counter_t _comms_errors; + perf_counter_t _dupe_count_perf; + + static constexpr uint64_t _collect_time{15000}; // 15 milliseconds, optical flow data publish rate + + uint64_t _previous_collect_timestamp{0}; + uint64_t _flow_dt_sum_usec{0}; + unsigned _frame_count_since_last{0}; + + enum Rotation _yaw_rotation {ROTATION_NONE}; + + int _flow_sum_x{0}; + int _flow_sum_y{0}; + + Mode _mode{Mode::LowLight}; + +}; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp new file mode 100644 index 000000000000..8dcbf626afa8 --- /dev/null +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * 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; // shared with the PMW3901 +static constexpr uint8_t REVISION_ID = 0x01; + +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 uint64_t T_SWW{11}; // 10.5 microseconds +static constexpr uint64_t T_SRR{2}; // 1.5 microseconds + +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, + +}; + +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/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp new file mode 100644 index 000000000000..ce8e1c075e0a --- /dev/null +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 "PAW3902.hpp" + +/** + * Local functions in support of the shell command. + */ +namespace pmw3902 +{ + +PAW3902 *g_dev; + +void start(int spi_bus); +void stop(); +void test(); +void reset(); +void info(); +void usage(); + + +/** + * Start the driver. + */ +void +start(int spi_bus) +{ + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new PAW3902(spi_bus, (enum Rotation)0); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +/** + * Print a little info about how to start/stop/use the driver + */ +void usage() +{ + PX4_INFO("usage: pmw3902 {start|test|reset|info'}"); + PX4_INFO(" [-b SPI_BUS]"); +} + +} // namespace pmw3902 + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int paw3902_main(int argc, char *argv[]); + +int +paw3902_main(int argc, char *argv[]) +{ + if (argc < 2) { + pmw3902::usage(); + return PX4_ERROR; + } + + // don't exit from getopt loop to leave getopt global variables in consistent state, + // set error flag instead + bool err_flag = false; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + int spi_bus = PAW3902_BUS; + + while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + spi_bus = (uint8_t)atoi(myoptarg); + + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + pmw3902::usage(); + return PX4_ERROR; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + pmw3902::start(spi_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + pmw3902::stop(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "status")) { + pmw3902::info(); + } + + pmw3902::usage(); + return PX4_ERROR; +} diff --git a/src/drivers/optical_flow/pmw3901/CMakeLists.txt b/src/drivers/optical_flow/pmw3901/CMakeLists.txt index c41c197c7dc7..33bb3c451120 100644 --- a/src/drivers/optical_flow/pmw3901/CMakeLists.txt +++ b/src/drivers/optical_flow/pmw3901/CMakeLists.txt @@ -32,11 +32,10 @@ ############################################################################ px4_add_module( - MODULE drivers__pmw3901 + MODULE drivers__optical_flow__pmw3901 MAIN pmw3901 - COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable - STACK_MAIN 1400 SRCS - pmw3901.cpp + pmw3901_main.cpp + PMW3901.cpp + PMW3901.hpp ) diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp new file mode 100644 index 000000000000..b4df95cb3eca --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -0,0 +1,414 @@ +/**************************************************************************** + * + * 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 "PMW3901.hpp" + +static constexpr uint32_t TIME_us_TSWW = 11; // - actually 10.5us + +PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : + SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901: read")), + _comms_errors(perf_alloc(PC_COUNT, "pmw3901: com err")), + _yaw_rotation(yaw_rotation) +{ +} + +PMW3901::~PMW3901() +{ + // make sure we are truly inactive + stop(); + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int +PMW3901::sensorInit() +{ + uint8_t data[5] {}; + + // Power on reset + writeRegister(0x3A, 0x5A); + usleep(5000); + + // Reading the motion registers one time + readRegister(0x02, &data[0], 1); + readRegister(0x03, &data[1], 1); + readRegister(0x04, &data[2], 1); + readRegister(0x05, &data[3], 1); + readRegister(0x06, &data[4], 1); + + usleep(1000); + + // set performance optimization registers + // from PixArt PMW3901MB Optical Motion Tracking chip demo kit V3.20 (21 Aug 2018) + unsigned char v = 0; + unsigned char c1 = 0; + unsigned char c2 = 0; + + writeRegister(0x7F, 0x00); + writeRegister(0x55, 0x01); + writeRegister(0x50, 0x07); + writeRegister(0x7f, 0x0e); + writeRegister(0x43, 0x10); + + readRegister(0x67, &v, 1); + + // if bit7 is set + if (v & (1 << 7)) { + writeRegister(0x48, 0x04); + + } else { + writeRegister(0x48, 0x02); + } + + writeRegister(0x7F, 0x00); + writeRegister(0x51, 0x7b); + writeRegister(0x50, 0x00); + writeRegister(0x55, 0x00); + + writeRegister(0x7F, 0x0e); + readRegister(0x73, &v, 1); + + if (v == 0) { + readRegister(0x70, &c1, 1); + + if (c1 <= 28) { + c1 = c1 + 14; + + } else { + c1 = c1 + 11; + } + + if (c1 > 0x3F) { + c1 = 0x3F; + } + + readRegister(0x71, &c2, 1); + c2 = ((unsigned short)c2 * 45) / 100; + + writeRegister(0x7f, 0x00); + writeRegister(0x61, 0xAD); + writeRegister(0x51, 0x70); + writeRegister(0x7f, 0x0e); + writeRegister(0x70, c1); + writeRegister(0x71, c2); + } + + writeRegister(0x7F, 0x00); + writeRegister(0x61, 0xAD); + writeRegister(0x7F, 0x03); + writeRegister(0x40, 0x00); + writeRegister(0x7F, 0x05); + writeRegister(0x41, 0xB3); + writeRegister(0x43, 0xF1); + writeRegister(0x45, 0x14); + writeRegister(0x5B, 0x32); + writeRegister(0x5F, 0x34); + writeRegister(0x7B, 0x08); + writeRegister(0x7F, 0x06); + writeRegister(0x44, 0x1B); + writeRegister(0x40, 0xBF); + writeRegister(0x4E, 0x3F); + writeRegister(0x7F, 0x08); + writeRegister(0x65, 0x20); + writeRegister(0x6A, 0x18); + writeRegister(0x7F, 0x09); + writeRegister(0x4F, 0xAF); + writeRegister(0x5F, 0x40); + writeRegister(0x48, 0x80); + writeRegister(0x49, 0x80); + writeRegister(0x57, 0x77); + writeRegister(0x60, 0x78); + writeRegister(0x61, 0x78); + writeRegister(0x62, 0x08); + writeRegister(0x63, 0x50); + writeRegister(0x7F, 0x0A); + writeRegister(0x45, 0x60); + writeRegister(0x7F, 0x00); + writeRegister(0x4D, 0x11); + writeRegister(0x55, 0x80); + writeRegister(0x74, 0x21); + writeRegister(0x75, 0x1F); + writeRegister(0x4A, 0x78); + writeRegister(0x4B, 0x78); + writeRegister(0x44, 0x08); + writeRegister(0x45, 0x50); + writeRegister(0x64, 0xFF); + writeRegister(0x65, 0x1F); + writeRegister(0x7F, 0x14); + writeRegister(0x65, 0x67); + writeRegister(0x66, 0x08); + writeRegister(0x63, 0x70); + writeRegister(0x7F, 0x15); + writeRegister(0x48, 0x48); + writeRegister(0x7F, 0x07); + writeRegister(0x41, 0x0D); + writeRegister(0x43, 0x14); + writeRegister(0x4B, 0x0E); + writeRegister(0x45, 0x0F); + writeRegister(0x44, 0x42); + writeRegister(0x4C, 0x80); + writeRegister(0x7F, 0x10); + writeRegister(0x5B, 0x02); + writeRegister(0x7F, 0x07); + writeRegister(0x40, 0x41); + writeRegister(0x70, 0x00); + + px4_usleep(10000); // delay 10ms + + writeRegister(0x32, 0x44); + writeRegister(0x7F, 0x07); + writeRegister(0x40, 0x40); + writeRegister(0x7F, 0x06); + writeRegister(0x62, 0xF0); + writeRegister(0x63, 0x00); + writeRegister(0x7F, 0x0D); + writeRegister(0x48, 0xC0); + writeRegister(0x6F, 0xD5); + writeRegister(0x7F, 0x00); + writeRegister(0x5B, 0xA0); + writeRegister(0x4E, 0xA8); + writeRegister(0x5A, 0x50); + writeRegister(0x40, 0x80); + + return PX4_OK; +} + +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); + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + return PX4_ERROR; + } + + sensorInit(); + + _previous_collect_timestamp = hrt_absolute_time(); + + start(); + + return PX4_OK; +} + +int +PMW3901::probe() +{ + uint8_t data[2] {}; + + readRegister(0x00, &data[0], 1); // chip id + + // Test the SPI communication, checking chipId and inverse chipId + if (data[0] == 0x49) { + return OK; + } + + // not found on any address + return -EIO; +} + +int +PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) +{ + uint8_t cmd[5]; // read up to 4 bytes + + cmd[0] = DIR_READ(reg); + + int ret = transfer(&cmd[0], &cmd[0], count + 1); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_LOG("spi::transfer returned %d", ret); + return ret; + } + + memcpy(&data[0], &cmd[1], count); + + return ret; +} + +int +PMW3901::writeRegister(unsigned reg, uint8_t data) +{ + uint8_t cmd[2]; // write 1 byte + int ret; + + cmd[0] = DIR_WRITE(reg); + cmd[1] = data; + + ret = transfer(&cmd[0], nullptr, 2); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_LOG("spi::transfer returned %d", ret); + return ret; + } + + px4_usleep(TIME_us_TSWW); + + return ret; +} + +void +PMW3901::Run() +{ + perf_begin(_sample_perf); + + int16_t delta_x_raw = 0; + int16_t delta_y_raw = 0; + uint8_t qual = 0; + float delta_x = 0.0f; + float delta_y = 0.0f; + + uint64_t timestamp = hrt_absolute_time(); + uint64_t dt_flow = timestamp - _previous_collect_timestamp; + _previous_collect_timestamp = timestamp; + + _flow_dt_sum_usec += dt_flow; + + readMotionCount(delta_x_raw, delta_y_raw, qual); + + _flow_sum_x += delta_x_raw; + _flow_sum_y += delta_y_raw; + + // returns if the collect time has not been reached + if (_flow_dt_sum_usec < _collect_time) { + return; + } + + delta_x = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians + delta_y = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians + + optical_flow_s report{}; + report.timestamp = timestamp; + + report.pixel_flow_x_integral = static_cast(delta_x); + report.pixel_flow_y_integral = static_cast(delta_y); + + // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + 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); + + report.frame_count_since_last_readout = 4; // microseconds + report.integration_timespan = _flow_dt_sum_usec; // microseconds + + report.sensor_id = 0; + + // This sensor doesn't provide any quality metric. However if the sensor is unable to calculate the optical flow it will + // output 0 for the delta. Hence, we set the measurement to "invalid" (quality = 0) if the values are smaller than FLT_EPSILON + if (fabsf(report.pixel_flow_x_integral) < FLT_EPSILON && fabsf(report.pixel_flow_y_integral) < FLT_EPSILON) { + report.quality = 0; + + } else { + report.quality = qual; + } + + /* No gyro on this board */ + report.gyro_x_rate_integral = NAN; + report.gyro_y_rate_integral = NAN; + report.gyro_z_rate_integral = NAN; + + // set (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 + + _flow_dt_sum_usec = 0; + _flow_sum_x = 0; + _flow_sum_y = 0; + + _optical_flow_pub.publish(report); + + perf_end(_sample_perf); +} + +int +PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual) +{ + uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, + DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0 + }; + + int ret = transfer(&data[0], &data[0], 12); + + if (OK != ret) { + perf_count(_comms_errors); + DEVICE_LOG("spi::transfer returned %d", ret); + return ret; + } + + deltaX = ((int16_t)data[5] << 8) | data[3]; + deltaY = ((int16_t)data[9] << 8) | data[7]; + qual = data[11]; + + ret = OK; + + return ret; +} + +void +PMW3901::start() +{ + // schedule a cycle to start things + ScheduleOnInterval(PMW3901_SAMPLE_INTERVAL, PMW3901_US); +} + +void +PMW3901::stop() +{ + ScheduleClear(); +} + +void +PMW3901::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp new file mode 100644 index 000000000000..09d5768baeb5 --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file PMW3901.hpp + * @author Daniele Pettenuzzo + * + * Driver for the pmw3901 optical flow sensor connected via SPI. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* Configuration Constants */ + +#if defined PX4_SPI_BUS_EXPANSION // crazyflie +# define PMW3901_BUS PX4_SPI_BUS_EXPANSION +#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5 +# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1 +#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi +# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL +#else +# error "add the required spi bus from board_config.h here" +#endif + +#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck +# define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 +#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1 +# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1 +#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi +# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL +#else +# error "add the required spi dev from board_config.h here" +#endif + +#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz + +#define DIR_WRITE(a) ((a) | (1 << 7)) +#define DIR_READ(a) ((a) & 0x7f) + +#define PMW3901_DEVICE_PATH "/dev/pmw3901" + +/* PMW3901 Registers addresses */ +#define PMW3901_US 1000 /* 1 ms */ +#define PMW3901_SAMPLE_INTERVAL 10000 /* 10 ms */ + + +class PMW3901 : public device::SPI, public px4::ScheduledWorkItem +{ +public: + PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0); + + virtual ~PMW3901(); + + virtual int init(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + const uint64_t _collect_time{15000}; // usecs, optical flow data publish rate + + uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + + uint64_t _previous_collect_timestamp{0}; + + enum Rotation _yaw_rotation; + + int _flow_sum_x{0}; + int _flow_sum_y{0}; + uint64_t _flow_dt_sum_usec{0}; + + /** + * 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(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void Run() override; + + int readRegister(unsigned reg, uint8_t *data, unsigned count); + int writeRegister(unsigned reg, uint8_t data); + + int sensorInit(); + int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual); +}; diff --git a/src/drivers/optical_flow/pmw3901/parameters.c b/src/drivers/optical_flow/pmw3901/parameters.c new file mode 100644 index 000000000000..74e9a482663d --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * PMW3901 Optical Flow + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_PMW3901, 0); diff --git a/src/drivers/optical_flow/pmw3901/pmw3901.cpp b/src/drivers/optical_flow/pmw3901/pmw3901.cpp deleted file mode 100644 index b7cfec44a831..000000000000 --- a/src/drivers/optical_flow/pmw3901/pmw3901.cpp +++ /dev/null @@ -1,915 +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 pmw3901.cpp - * @author Daniele Pettenuzzo - * - * Driver for the pmw3901 optical flow sensor connected via SPI. - */ - -#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 - -/* Configuration Constants */ - -#if defined PX4_SPI_BUS_EXPANSION // crazyflie -# define PMW3901_BUS PX4_SPI_BUS_EXPANSION -#elif defined PX4_SPI_BUS_EXTERNAL1 // fmu-v5 -# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL1 -#elif defined PX4_SPI_BUS_EXTERNAL // fmu-v4 extspi -# define PMW3901_BUS PX4_SPI_BUS_EXTERNAL -#else -# error "add the required spi bus from board_config.h here" -#endif - -#if defined PX4_SPIDEV_EXPANSION_2 // crazyflie flow deck -# define PMW3901_SPIDEV PX4_SPIDEV_EXPANSION_2 -#elif defined PX4_SPIDEV_EXTERNAL1_1 // fmu-v5 ext CS1 -# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL1_1 -#elif defined PX4_SPIDEV_EXTERNAL // fmu-v4 extspi -# define PMW3901_SPIDEV PX4_SPIDEV_EXTERNAL -#else -# error "add the required spi dev from board_config.h here" -#endif - -#define PMW3901_SPI_BUS_SPEED (2000000L) // 2MHz - -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define DIR_READ(a) ((a) & 0x7f) - -#define PMW3901_DEVICE_PATH "/dev/pmw3901" - -/* PMW3901 Registers addresses */ -#define PMW3901_US 1000 /* 1 ms */ -#define PMW3901_SAMPLE_RATE 10000 /* 10 ms */ - - -class PMW3901 : public device::SPI, public px4::ScheduledWorkItem -{ -public: - PMW3901(int bus = PMW3901_BUS, enum Rotation yaw_rotation = (enum Rotation)0); - - virtual ~PMW3901(); - - virtual int init(); - - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - ringbuffer::RingBuffer *_reports; - bool _sensor_ok; - int _measure_interval; - int _class_instance; - int _orb_class_instance; - - const uint64_t _collect_time = 15000; // usecs, optical flow data publish rate - - orb_advert_t _optical_flow_pub; - orb_advert_t _subsystem_pub; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - uint64_t _previous_collect_timestamp; - - enum Rotation _yaw_rotation; - int _flow_sum_x = 0; - int _flow_sum_y = 0; - uint64_t _flow_dt_sum_usec = 0; - - - /** - * 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(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void Run() override; - - int collect(); - - int readRegister(unsigned reg, uint8_t *data, unsigned count); - int writeRegister(unsigned reg, uint8_t data); - - int sensorInit(); - int readMotionCount(int16_t &deltaX, int16_t &deltaY); -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]); - -PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : - SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), - ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), - _reports(nullptr), - _sensor_ok(false), - _measure_interval(0), - _class_instance(-1), - _orb_class_instance(-1), - _optical_flow_pub(nullptr), - _subsystem_pub(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901_read")), - _comms_errors(perf_alloc(PC_COUNT, "pmw3901_com_err")), - _previous_collect_timestamp(0), - _yaw_rotation(yaw_rotation) -{ -} - -PMW3901::~PMW3901() -{ - /* make sure we are truly inactive */ - stop(); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); -} - - -int -PMW3901::sensorInit() -{ - uint8_t data[5]; - - // Power on reset - writeRegister(0x3A, 0x5A); - usleep(5000); - - // Reading the motion registers one time - readRegister(0x02, &data[0], 1); - readRegister(0x03, &data[1], 1); - readRegister(0x04, &data[2], 1); - readRegister(0x05, &data[3], 1); - readRegister(0x06, &data[4], 1); - - usleep(1000); - - // set performance optimization registers - writeRegister(0x7F, 0x00); - writeRegister(0x61, 0xAD); - writeRegister(0x7F, 0x03); - writeRegister(0x40, 0x00); - writeRegister(0x7F, 0x05); - writeRegister(0x41, 0xB3); - writeRegister(0x43, 0xF1); - writeRegister(0x45, 0x14); - writeRegister(0x5B, 0x32); - writeRegister(0x5F, 0x34); - writeRegister(0x7B, 0x08); - writeRegister(0x7F, 0x06); - writeRegister(0x44, 0x1B); - writeRegister(0x40, 0xBF); - writeRegister(0x4E, 0x3F); - writeRegister(0x7F, 0x08); - writeRegister(0x65, 0x20); - writeRegister(0x6A, 0x18); - writeRegister(0x7F, 0x09); - writeRegister(0x4F, 0xAF); - writeRegister(0x5F, 0x40); - writeRegister(0x48, 0x80); - writeRegister(0x49, 0x80); - writeRegister(0x57, 0x77); - writeRegister(0x60, 0x78); - writeRegister(0x61, 0x78); - writeRegister(0x62, 0x08); - writeRegister(0x63, 0x50); - writeRegister(0x7F, 0x0A); - writeRegister(0x45, 0x60); - writeRegister(0x7F, 0x00); - writeRegister(0x4D, 0x11); - writeRegister(0x55, 0x80); - writeRegister(0x74, 0x1F); - writeRegister(0x75, 0x1F); - writeRegister(0x4A, 0x78); - writeRegister(0x4B, 0x78); - writeRegister(0x44, 0x08); - writeRegister(0x45, 0x50); - writeRegister(0x64, 0xFF); - writeRegister(0x65, 0x1F); - writeRegister(0x7F, 0x14); - writeRegister(0x65, 0x60); - writeRegister(0x66, 0x08); - writeRegister(0x63, 0x78); - writeRegister(0x7F, 0x15); - writeRegister(0x48, 0x58); - writeRegister(0x7F, 0x07); - writeRegister(0x41, 0x0D); - writeRegister(0x43, 0x14); - writeRegister(0x4B, 0x0E); - writeRegister(0x45, 0x0F); - writeRegister(0x44, 0x42); - writeRegister(0x4C, 0x80); - writeRegister(0x7F, 0x10); - writeRegister(0x5B, 0x02); - writeRegister(0x7F, 0x07); - writeRegister(0x40, 0x41); - writeRegister(0x70, 0x00); - - usleep(10000); - - writeRegister(0x32, 0x44); - writeRegister(0x7F, 0x07); - writeRegister(0x40, 0x40); - writeRegister(0x7F, 0x06); - writeRegister(0x62, 0xf0); - writeRegister(0x63, 0x00); - writeRegister(0x7F, 0x0D); - writeRegister(0x48, 0xC0); - writeRegister(0x6F, 0xd5); - writeRegister(0x7F, 0x00); - writeRegister(0x5B, 0xa0); - writeRegister(0x4E, 0xA8); - writeRegister(0x5A, 0x50); - writeRegister(0x40, 0x80); - - writeRegister(0x7F, 0x00); - writeRegister(0x5A, 0x10); - writeRegister(0x54, 0x00); - - return OK; - -} - -int -PMW3901::init() -{ - int ret = PX4_ERROR; - - // 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); - - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - goto out; - } - - sensorInit(); - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); - - if (_reports == nullptr) { - goto out; - } - - ret = OK; - _sensor_ok = true; - _previous_collect_timestamp = hrt_absolute_time(); - -out: - return ret; - -} - - -int -PMW3901::probe() -{ - uint8_t data[2] = { 0, 0 }; - - readRegister(0x00, &data[0], 1); // chip id - - // Test the SPI communication, checking chipId and inverse chipId - if (data[0] == 0x49) { - return OK; - } - - // not found on any address - return -EIO; -} - - -int -PMW3901::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - case 0: - return -EINVAL; - - case SENSOR_POLLRATE_DEFAULT: { - - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_interval = (PMW3901_SAMPLE_RATE); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_interval == 0); - - /* convert hz to tick interval via microseconds */ - unsigned interval = (1000000 / arg); - - /* check against maximum rate */ - if (interval < (PMW3901_SAMPLE_RATE)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_interval = interval; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - default: - /* give it to the superclass */ - return SPI::ioctl(filp, cmd, arg); - } -} - -ssize_t -PMW3901::read(device::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_interval > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(rbuf)) { - ret += sizeof(*rbuf); - rbuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(rbuf)) { - ret = sizeof(*rbuf); - } - - } while (0); - - return ret; -} - - -int -PMW3901::readRegister(unsigned reg, uint8_t *data, unsigned count) -{ - uint8_t cmd[5]; // read up to 4 bytes - int ret; - - cmd[0] = DIR_READ(reg); - - ret = transfer(&cmd[0], &cmd[0], count + 1); - - if (OK != ret) { - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; - } - - memcpy(&data[0], &cmd[1], count); - - return ret; - -} - - -int -PMW3901::writeRegister(unsigned reg, uint8_t data) -{ - uint8_t cmd[2]; // write 1 byte - int ret; - - cmd[0] = DIR_WRITE(reg); - cmd[1] = data; - - ret = transfer(&cmd[0], nullptr, 2); - - if (OK != ret) { - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; - } - - return ret; - -} - - -int -PMW3901::collect() -{ - int ret = OK; - int16_t delta_x_raw, delta_y_raw; - float delta_x, delta_y; - - perf_begin(_sample_perf); - - uint64_t timestamp = hrt_absolute_time(); - uint64_t dt_flow = timestamp - _previous_collect_timestamp; - _previous_collect_timestamp = timestamp; - - _flow_dt_sum_usec += dt_flow; - - readMotionCount(delta_x_raw, delta_y_raw); - - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < _collect_time) { - - return ret; - } - - delta_x = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians - delta_y = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians - - struct optical_flow_s report; - - report.timestamp = timestamp; - - report.pixel_flow_x_integral = static_cast(delta_x); - report.pixel_flow_y_integral = static_cast(delta_y); - - // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT - 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); - - report.frame_count_since_last_readout = 4; //microseconds - report.integration_timespan = _flow_dt_sum_usec; //microseconds - - report.sensor_id = 0; - - // This sensor doesn't provide any quality metric. However if the sensor is unable to calculate the optical flow it will - // output 0 for the delta. Hence, we set the measurement to "invalid" (quality = 0) if the values are smaller than FLT_EPSILON - if (fabsf(report.pixel_flow_x_integral) < FLT_EPSILON && fabsf(report.pixel_flow_y_integral) < FLT_EPSILON) { - report.quality = 0; - - } else { - report.quality = 255; - } - - /* No gyro on this board */ - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; - - // set (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 - - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - - if (_optical_flow_pub == nullptr) { - - _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &report); - - } else { - - orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &report); - } - - /* post a report to the ring */ - _reports->force(&report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - perf_end(_sample_perf); - - return ret; - -} - - -int -PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY) -{ - int ret; - - uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0, - DIR_READ(0x05), 0, DIR_READ(0x06), 0 - }; - - ret = transfer(&data[0], &data[0], 10); - - if (OK != ret) { - perf_count(_comms_errors); - DEVICE_LOG("spi::transfer returned %d", ret); - return ret; - } - - deltaX = ((int16_t)data[5] << 8) | data[3]; - deltaY = ((int16_t)data[9] << 8) | data[7]; - - ret = OK; - - return ret; - -} - - -void -PMW3901::start() -{ - /* reset the report ring and state machine */ - _reports->flush(); - - /* schedule a cycle to start things */ - ScheduleDelayed(PMW3901_US); - - /* notify about state change */ - struct subsystem_info_s info = {}; - - info.timestamp = hrt_absolute_time(); - info.present = true; - info.enabled = true; - info.ok = true; - info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW; - - if (_subsystem_pub != nullptr) { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); - - } else { - _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); - } -} - -void -PMW3901::stop() -{ - ScheduleClear(); -} - -void -PMW3901::Run() -{ - collect(); - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(PMW3901_SAMPLE_RATE); -} - -void -PMW3901::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - printf("poll interval: %u\n", _measure_interval); - _reports->print_info("report queue"); -} - -/** - * Local functions in support of the shell command. - */ -namespace pmw3901 -{ - -PMW3901 *g_dev; - -void start(int spi_bus); -void stop(); -void test(); -void reset(); -void info(); -void usage(); - - -/** - * Start the driver. - */ -void -start(int spi_bus) -{ - int fd; - - if (g_dev != nullptr) { - errx(1, "already started"); - } - - /* create the driver */ - g_dev = new PMW3901(spi_bus, (enum Rotation)0); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - fd = open(PMW3901_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - exit(0); - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Stop the driver - */ -void stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - errx(1, "driver not running"); - } - - exit(0); -} - - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -void -test() -{ - - struct optical_flow_s report; - ssize_t sz; - - int fd = open(PMW3901_DEVICE_PATH, O_RDONLY); - - if (fd < 0) { - err(1, "%s open failed (try 'pmw3901 start' if the driver is not running)", PMW3901_DEVICE_PATH); - } - - /* do a simple demand read */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("ret: %d, expected: %d", sz, sizeof(report)); - err(1, "immediate acc read failed"); - } - - print_message(report); - - errx(0, "PASS"); -} - - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - errx(1, "driver not running"); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -/** - * Print a little info about how to start/stop/use the driver - */ -void usage() -{ - PX4_INFO("usage: pmw3901 {start|test|reset|info'}"); - PX4_INFO(" [-b SPI_BUS]"); -} - -} // namespace pmw3901 - - -int -pmw3901_main(int argc, char *argv[]) -{ - if (argc < 2) { - pmw3901::usage(); - return PX4_ERROR; - } - - // don't exit from getopt loop to leave getopt global variables in consistent state, - // set error flag instead - bool err_flag = false; - int ch; - int myoptind = 1; - const char *myoptarg = nullptr; - int spi_bus = PMW3901_BUS; - - while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'b': - spi_bus = (uint8_t)atoi(myoptarg); - - break; - - default: - err_flag = true; - break; - } - } - - if (err_flag) { - pmw3901::usage(); - return PX4_ERROR; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[myoptind], "start")) { - pmw3901::start(spi_bus); - } - - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { - pmw3901::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[myoptind], "test")) { - pmw3901::test(); - } - - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { - pmw3901::info(); - } - - pmw3901::usage(); - return PX4_ERROR; -} diff --git a/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp new file mode 100644 index 000000000000..3c59adc3b246 --- /dev/null +++ b/src/drivers/optical_flow/pmw3901/pmw3901_main.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 "PMW3901.hpp" + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int pmw3901_main(int argc, char *argv[]); + +/** + * Local functions in support of the shell command. + */ +namespace pmw3901 +{ + +PMW3901 *g_dev; + +void start(int spi_bus); +void stop(); +void test(); +void reset(); +void info(); +void usage(); + +/** + * Start the driver. + */ +void +start(int spi_bus) +{ + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new PMW3901(spi_bus, (enum Rotation)0); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +/** + * Print a little info about how to start/stop/use the driver + */ +void usage() +{ + PX4_INFO("usage: pmw3901 {start|test|reset|info'}"); + PX4_INFO(" [-b SPI_BUS]"); +} + +} // namespace pmw3901 + + +int +pmw3901_main(int argc, char *argv[]) +{ + if (argc < 2) { + pmw3901::usage(); + return PX4_ERROR; + } + + // don't exit from getopt loop to leave getopt global variables in consistent state, + // set error flag instead + bool err_flag = false; + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + int spi_bus = PMW3901_BUS; + + while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + spi_bus = (uint8_t)atoi(myoptarg); + + break; + + default: + err_flag = true; + break; + } + } + + if (err_flag) { + pmw3901::usage(); + return PX4_ERROR; + } + + /* + * Start/load the driver. + */ + if (!strcmp(argv[myoptind], "start")) { + pmw3901::start(spi_bus); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[myoptind], "stop")) { + pmw3901::stop(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + pmw3901::info(); + } + + pmw3901::usage(); + return PX4_ERROR; +} diff --git a/src/drivers/optical_flow/px4flow/CMakeLists.txt b/src/drivers/optical_flow/px4flow/CMakeLists.txt index b169a5a9d3a0..e3928d80d1a0 100644 --- a/src/drivers/optical_flow/px4flow/CMakeLists.txt +++ b/src/drivers/optical_flow/px4flow/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__px4flow MAIN px4flow - STACK_MAIN 1500 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/drivers/optical_flow/px4flow/parameters.c b/src/drivers/optical_flow/px4flow/parameters.c new file mode 100644 index 000000000000..485dd08416ec --- /dev/null +++ b/src/drivers/optical_flow/px4flow/parameters.c @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * PX4 Flow Optical Flow + * + * @reboot_required true + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_EN_PX4FLOW, 0); diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index 789b2b5c0485..e65f69465fb7 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index 4b6edb950c0c..9af6e441b996 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -46,7 +46,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp index adaa6733c04c..6b3ef7307eb9 100644 --- a/src/drivers/pca9685/pca9685.cpp +++ b/src/drivers/pca9685/pca9685.cpp @@ -63,7 +63,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 034b7ed8a244..b1e740abee6b 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include /* Configuration Constants */ #define INA226_BUS_DEFAULT PX4_I2C_BUS_EXPANSION diff --git a/src/drivers/protocol_splitter/CMakeLists.txt b/src/drivers/protocol_splitter/CMakeLists.txt index ebcad6f89d64..de6865adf601 100644 --- a/src/drivers/protocol_splitter/CMakeLists.txt +++ b/src/drivers/protocol_splitter/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__protocol_splitter MAIN protocol_splitter - STACK_MAIN 1200 SRCS protocol_splitter.cpp DEPENDS diff --git a/src/drivers/pwm_input/CMakeLists.txt b/src/drivers/pwm_input/CMakeLists.txt index 3079933e78b8..e3e8a7d32cd3 100644 --- a/src/drivers/pwm_input/CMakeLists.txt +++ b/src/drivers/pwm_input/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__pwm_input MAIN pwm_input - STACK_MAIN 1024 COMPILE_FLAGS -Wno-pmf-conversions -Wno-cast-align # TODO: fix and enable diff --git a/src/drivers/pwm_out_sim/PWMSim.cpp b/src/drivers/pwm_out_sim/PWMSim.cpp index 3aea68c91509..4f5c0934f053 100644 --- a/src/drivers/pwm_out_sim/PWMSim.cpp +++ b/src/drivers/pwm_out_sim/PWMSim.cpp @@ -161,9 +161,6 @@ PWMSim::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* advertise the mixed control outputs, insist on the first group output */ - _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_actuator_outputs); - update_params(); int params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -293,9 +290,10 @@ PWMSim::run() orb_publish_auto(ORB_ID(multirotor_motor_limits), &_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT); } - /* and publish for anyone that cares to see */ _actuator_outputs.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_actuator_outputs); + + _outputs_pub.publish(_actuator_outputs); + // use first valid timestamp_sample for latency tracking for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { diff --git a/src/drivers/pwm_out_sim/PWMSim.hpp b/src/drivers/pwm_out_sim/PWMSim.hpp index a8d9ec7ecf01..802f5a450c14 100644 --- a/src/drivers/pwm_out_sim/PWMSim.hpp +++ b/src/drivers/pwm_out_sim/PWMSim.hpp @@ -41,11 +41,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -110,8 +110,8 @@ class PWMSim : public cdev::CDev, public ModuleBase int _armed_sub{-1}; - actuator_outputs_s _actuator_outputs = {}; - orb_advert_t _outputs_pub{nullptr}; + actuator_outputs_s _actuator_outputs {}; + uORB::Publication _outputs_pub{ORB_ID(actuator_outputs)}; orb_advert_t _mixer_status{nullptr}; unsigned _num_outputs{0}; diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt index 1f04bc13cedd..d57c0148cf87 100644 --- a/src/drivers/px4fmu/CMakeLists.txt +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -33,12 +33,13 @@ px4_add_module( MODULE drivers__px4fmu MAIN fmu - STACK_MAIN 1200 COMPILE_FLAGS SRCS fmu.cpp DEPENDS + arch_io_pins circuit_breaker mixer - pwm_limit + mixer_module + output_limit ) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 61e7e0435e49..884530668db1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * 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 @@ -47,36 +47,28 @@ #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 - -#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ -static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */ - -/* - * Define the various LED flash sequences for each system state. - */ -#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */ -#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */ -#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */ -#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */ -#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */ +using namespace time_literals; /** Mode given via CLI */ enum PortMode { @@ -104,7 +96,8 @@ enum PortMode { #define PX4FMU_DEVICE_PATH "/dev/px4fmu" -class PX4FMU : public cdev::CDev, public ModuleBase + +class PX4FMU : public cdev::CDev, public ModuleBase, public OutputModuleInterface { public: enum Mode { @@ -126,7 +119,7 @@ class PX4FMU : public cdev::CDev, public ModuleBase MODE_5CAP, MODE_6CAP, }; - PX4FMU(bool run_as_task); + PX4FMU(); virtual ~PX4FMU(); /** @see ModuleBase */ @@ -141,13 +134,7 @@ class PX4FMU : public cdev::CDev, public ModuleBase /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; - - /** - * run the main loop: if running as task, continuously iterate, otherwise execute only one single cycle - */ - void cycle(); + void Run() override; /** @see ModuleBase::print_status() */ int print_status() override; @@ -160,7 +147,6 @@ class PX4FMU : public cdev::CDev, public ModuleBase static int fake(int argc, char *argv[]); virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual ssize_t write(file *filp, const char *buffer, size_t len); virtual int init(); @@ -175,88 +161,41 @@ class PX4FMU : public cdev::CDev, public ModuleBase void update_pwm_trims(); + void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + private: + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, true}; - enum class MotorOrdering : int32_t { - PX4 = 0, - Betaflight = 1 - }; + static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; - hrt_abstime _cycle_timestamp = 0; - hrt_abstime _last_safety_check = 0; - hrt_abstime _time_last_mix = 0; - - static constexpr unsigned _MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; - - Mode _mode; - unsigned _pwm_default_rate; - unsigned _pwm_alt_rate; - uint32_t _pwm_alt_rate_channels; - unsigned _current_update_rate; - bool _run_as_task; - static struct work_s _work; - - int _armed_sub; - int _param_sub; - int _safety_sub; - - orb_advert_t _outputs_pub; - unsigned _num_outputs; - int _class_instance; - - bool _throttle_armed; - bool _pwm_on; - uint32_t _pwm_mask; - bool _pwm_initialized; - bool _test_mode; - - MixerGroup *_mixers; - - uint32_t _groups_required; - uint32_t _groups_subscribed; - int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {-1, -1, -1, -1}; - actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; - unsigned _poll_fds_num; - - static pwm_limit_t _pwm_limit; - static actuator_armed_s _armed; - uint16_t _failsafe_pwm[_MAX_ACTUATORS] {}; - uint16_t _disarmed_pwm[_MAX_ACTUATORS] {}; - uint16_t _min_pwm[_MAX_ACTUATORS] {}; - uint16_t _max_pwm[_MAX_ACTUATORS] {}; - uint16_t _reverse_pwm_mask; - unsigned _num_failsafe_set; - unsigned _num_disarmed_set; - bool _safety_off; ///< State of the safety button from the subscribed safety topic - bool _safety_btn_off; ///< State of the safety button read from the HW button - bool _safety_disabled; - orb_advert_t _to_safety; - orb_advert_t _to_mixer_status; ///< mixer status flags - - float _mot_t_max; ///< maximum rise time for motor (slew rate limiting) - float _thr_mdl_fac; ///< thrust to pwm modelling factor - Mixer::Airmode _airmode; ///< multicopter air-mode - MotorOrdering _motor_ordering; - - perf_counter_t _perf_control_latency; - - static bool arm_nothrottle() - { - return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode); - } + Mode _mode{MODE_NONE}; + + unsigned _pwm_default_rate{50}; + unsigned _pwm_alt_rate{50}; + uint32_t _pwm_alt_rate_channels{0}; - static void cycle_trampoline(void *arg); - int start(); + unsigned _current_update_rate{0}; + + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + + + unsigned _num_outputs{0}; + int _class_instance{-1}; + + 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 _cycle_interval_perf; - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); - void subscribe(); + 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); void update_pwm_rev_mask(); @@ -272,123 +211,36 @@ class PX4FMU : public cdev::CDev, public ModuleBase PX4FMU(const PX4FMU &) = delete; PX4FMU operator=(const PX4FMU &) = delete; - void safety_check_button(void); - void flash_safety_button(void); - - /** - * Reorder PWM outputs according to _motor_ordering - * @param values PWM values to reorder - */ - inline void reorder_outputs(uint16_t values[_MAX_ACTUATORS]); }; -pwm_limit_t PX4FMU::_pwm_limit; -actuator_armed_s PX4FMU::_armed = {}; -work_s PX4FMU::_work = {}; - -PX4FMU::PX4FMU(bool run_as_task) : +PX4FMU::PX4FMU() : CDev(PX4FMU_DEVICE_PATH), - _mode(MODE_NONE), - _pwm_default_rate(50), - _pwm_alt_rate(50), - _pwm_alt_rate_channels(0), - _current_update_rate(0), - _run_as_task(run_as_task), - _armed_sub(-1), - _param_sub(-1), - _safety_sub(-1), - _outputs_pub(nullptr), - _num_outputs(0), - _class_instance(0), - _throttle_armed(false), - _pwm_on(false), - _pwm_mask(0), - _pwm_initialized(false), - _test_mode(false), - _mixers(nullptr), - _groups_required(0), - _groups_subscribed(0), - _poll_fds_num(0), - _reverse_pwm_mask(0), - _num_failsafe_set(0), - _num_disarmed_set(0), - _safety_off(false), - _safety_btn_off(false), - _safety_disabled(false), - _to_safety(nullptr), - _to_mixer_status(nullptr), - _mot_t_max(0.0f), - _thr_mdl_fac(0.0f), - _airmode(Mixer::Airmode::disabled), - _motor_ordering(MotorOrdering::PX4), - _perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency")) + OutputModuleInterface(px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")), + _cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval")) { - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - _min_pwm[i] = PWM_DEFAULT_MIN; - _max_pwm[i] = PWM_DEFAULT_MAX; - } - - _control_topics[0] = ORB_ID(actuator_controls_0); - _control_topics[1] = ORB_ID(actuator_controls_1); - _control_topics[2] = ORB_ID(actuator_controls_2); - _control_topics[3] = ORB_ID(actuator_controls_3); + _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); + _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; ++i) { - _control_subs[i] = -1; - } - - memset(_controls, 0, sizeof(_controls)); - memset(_poll_fds, 0, sizeof(_poll_fds)); - - // Safely initialize armed flags. - _armed.armed = false; - _armed.prearmed = false; - _armed.ready_to_arm = false; - _armed.lockdown = false; - _armed.force_failsafe = false; - _armed.in_esc_calibration_mode = false; - - // If there is no safety button, disable it on boot. -#ifndef GPIO_BTN_SAFETY - _safety_off = true; - _safety_btn_off = true; -#endif } PX4FMU::~PX4FMU() { - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_unsubscribe(_control_subs[i]); - } - } - - orb_unsubscribe(_armed_sub); - orb_unsubscribe(_param_sub); - orb_unsubscribe(_safety_sub); - - orb_unadvertise(_outputs_pub); - orb_unadvertise(_to_safety); - orb_unadvertise(_to_mixer_status); - /* make sure servos are off */ up_pwm_servo_deinit(); - /* note - someone else is responsible for restoring the GPIO config */ - /* clean up the alternate device node */ unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); - perf_free(_perf_control_latency); + perf_free(_cycle_perf); + perf_free(_cycle_interval_perf); } int PX4FMU::init() { - int ret; - /* do regular cdev init */ - ret = CDev::init(); + int ret = CDev::init(); if (ret != OK) { return ret; @@ -405,118 +257,15 @@ PX4FMU::init() PX4_ERR("FAILED registering class device"); } - _safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); - - if (_safety_disabled) { - _safety_off = true; - _safety_btn_off = true; - } - /* force a reset of the update rate */ _current_update_rate = 0; - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - _safety_sub = orb_subscribe(ORB_ID(safety)); - - /* initialize PWM limit lib */ - pwm_limit_init(&_pwm_limit); - // Getting initial parameter values update_params(); - return 0; -} + ScheduleNow(); -void -PX4FMU::safety_check_button(void) -{ -#ifdef GPIO_BTN_SAFETY - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - - static int counter = 0; - /* - * Debounce the safety button, change state if it has been held for long enough. - * - */ - bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); - - /* - * Keep pressed for a while to arm. - * - * Note that the counting sequence has to be same length - * for arming / disarming in order to end up as proper - * state machine, keep ARM_COUNTER_THRESHOLD the same - * length in all cases of the if/else struct below. - */ - if (safety_button_pressed && !_safety_btn_off) { - - if (counter < CYCLE_COUNT) { - counter++; - - } else if (counter == CYCLE_COUNT) { - /* switch to armed state */ - _safety_btn_off = true; - counter++; - } - - } else if (safety_button_pressed && _safety_btn_off) { - - if (counter < CYCLE_COUNT) { - counter++; - - } else if (counter == CYCLE_COUNT) { - /* change to disarmed state and notify the FMU */ - _safety_btn_off = false; - counter++; - } - - } else { - counter = 0; - } - } - -#endif -} - -void -PX4FMU::flash_safety_button() -{ -#if defined(GPIO_BTN_SAFETY) && defined(GPIO_LED_SAFETY) - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - /* Select the appropriate LED flash pattern depending on the current arm state */ - uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - - /* cycle the blink state machine at 10Hz */ - static int blink_counter = 0; - - if (_safety_btn_off) { - if (_armed.armed) { - pattern = LED_PATTERN_IO_FMU_ARMED; - - } else { - pattern = LED_PATTERN_IO_ARMED; - } - - } else if (_armed.armed) { - pattern = LED_PATTERN_FMU_ARMED; - - } else { - pattern = LED_PATTERN_FMU_OK_TO_ARM; - - } - - /* Turn the LED on if we have a 1 at the current bit position */ - px4_arch_gpiowrite(GPIO_LED_SAFETY, !(pattern & (1 << blink_counter++))); - - if (blink_counter > 15) { - blink_counter = 0; - } - } - -#endif + return 0; } int @@ -752,7 +501,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate for (unsigned pass = 0; pass < 2; pass++) { - /* We should note that group is iterated over from 0 to _MAX_ACTUATORS. + /* We should note that group is iterated over from 0 to MAX_ACTUATORS. * This allows for the ideal worlds situation: 1 channel per group * configuration. * @@ -766,7 +515,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate * rate and mode. (See rates above.) */ - for (unsigned group = 0; group < _MAX_ACTUATORS; group++) { + for (unsigned group = 0; group < MAX_ACTUATORS; group++) { // get the channel mask for this rate group uint32_t mask = up_pwm_servo_get_rate_group(group); @@ -808,6 +557,8 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate _pwm_default_rate = default_rate; _pwm_alt_rate = alt_rate; + _current_update_rate = 0; // force update + return OK; } @@ -818,37 +569,33 @@ PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz) } void -PX4FMU::subscribe() +PX4FMU::update_current_rate() { - /* subscribe/unsubscribe to required actuator control groups */ - uint32_t sub_groups = _groups_required & ~_groups_subscribed; - uint32_t unsub_groups = _groups_subscribed & ~_groups_required; - _poll_fds_num = 0; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (sub_groups & (1 << i)) { - PX4_DEBUG("subscribe to actuator_controls_%d", i); - _control_subs[i] = orb_subscribe(_control_topics[i]); - } - - if (unsub_groups & (1 << i)) { - PX4_DEBUG("unsubscribe from actuator_controls_%d", i); - orb_unsubscribe(_control_subs[i]); - _control_subs[i] = -1; - } + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + int max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - if (_control_subs[i] >= 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num++; - } + // oneshot + if ((_pwm_default_rate == 0) || (_pwm_alt_rate == 0)) { + max_rate = 2000; } + + // max interval 0.5 - 100 ms (10 - 2000Hz) + const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000); + + _current_update_rate = max_rate; + _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); } void PX4FMU::update_pwm_rev_mask() { - _reverse_pwm_mask = 0; + uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask(); + reverse_pwm_mask = 0; const char *pname_format; @@ -863,7 +610,7 @@ PX4FMU::update_pwm_rev_mask() return; } - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { char pname[16]; /* fill the channel reverse mask from parameters */ @@ -873,7 +620,7 @@ PX4FMU::update_pwm_rev_mask() if (param_h != PARAM_INVALID) { int32_t ival = 0; param_get(param_h, &ival); - _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; + reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; } } } @@ -883,137 +630,67 @@ PX4FMU::update_pwm_trims() { PX4_DEBUG("update_pwm_trims"); - if (_mixers != nullptr) { + if (!_mixing_output.mixers()) { + return; + } - int16_t values[_MAX_ACTUATORS] = {}; + int16_t values[MAX_ACTUATORS] = {}; - const char *pname_format; + const char *pname_format; - if (_class_instance == CLASS_DEVICE_PRIMARY) { - pname_format = "PWM_MAIN_TRIM%d"; + if (_class_instance == CLASS_DEVICE_PRIMARY) { + pname_format = "PWM_MAIN_TRIM%d"; - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - pname_format = "PWM_AUX_TRIM%d"; + } else if (_class_instance == CLASS_DEVICE_SECONDARY) { + pname_format = "PWM_AUX_TRIM%d"; - } else { - PX4_ERR("PWM TRIM only for MAIN and AUX"); - return; - } + } else { + PX4_ERR("PWM TRIM only for MAIN and AUX"); + return; + } - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - char pname[16]; + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + char pname[16]; - /* fill the struct from parameters */ - sprintf(pname, pname_format, i + 1); - param_t param_h = param_find(pname); + /* fill the struct from parameters */ + sprintf(pname, pname_format, i + 1); + param_t param_h = param_find(pname); - if (param_h != PARAM_INVALID) { - float pval = 0.0f; - param_get(param_h, &pval); - values[i] = (int16_t)(10000 * pval); - PX4_DEBUG("%s: %d", pname, values[i]); - } + if (param_h != PARAM_INVALID) { + float pval = 0.0f; + param_get(param_h, &pval); + values[i] = (int16_t)(10000 * pval); + PX4_DEBUG("%s: %d", pname, values[i]); } - - /* copy the trim values to the mixer offsets */ - unsigned n_out = _mixers->set_trims(values, _MAX_ACTUATORS); - PX4_DEBUG("set %d trims", n_out); } + + /* copy the trim values to the mixer offsets */ + unsigned n_out = _mixing_output.mixers()->set_trims(values, MAX_ACTUATORS); + PX4_DEBUG("set %d trims", n_out); } int PX4FMU::task_spawn(int argc, char *argv[]) { - bool run_as_task = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "t", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 't': - run_as_task = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return -1; - } - - - if (!run_as_task) { - - /* schedule a cycle to start things */ - int ret = work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, nullptr, 0); - - if (ret < 0) { - return ret; - } + PX4FMU *instance = new PX4FMU(); + if (instance) { + _object.store(instance); _task_id = task_id_is_work_queue; - } else { - - /* start the IO interface task */ - - _task_id = px4_task_spawn_cmd("fmu", - SCHED_DEFAULT, - SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 1340, - (px4_main_t)&run_trampoline, - nullptr); - - if (_task_id < 0) { - _task_id = -1; - return -errno; + if (instance->init() == PX4_OK) { + return PX4_OK; } - } - // wait until task is up & running (the mode_* commands depend on it) - if (wait_until_running() < 0) { - _task_id = -1; - return -1; + } else { + PX4_ERR("alloc failed"); } - return PX4_OK; -} - -void -PX4FMU::cycle_trampoline(void *arg) -{ - PX4FMU *dev = reinterpret_cast(arg); - - // check if the trampoline is called for the first time - if (!dev) { - dev = new PX4FMU(false); - - if (!dev) { - PX4_ERR("alloc failed"); - return; - } - - if (dev->init() != 0) { - PX4_ERR("init failed"); - delete dev; - return; - } - - _object.store(dev); - } + delete instance; + _object.store(nullptr); + _task_id = -1; - dev->cycle(); + return PX4_ERROR; } void @@ -1043,400 +720,79 @@ PX4FMU::update_pwm_out_state(bool on) up_pwm_servo_arm(on); } -void -PX4FMU::run() + +void PX4FMU::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) { - if (init() != 0) { - PX4_ERR("init failed"); - exit_and_cleanup(); + if (_test_mode) { return; } - cycle(); + /* output to the servos */ + if (_pwm_initialized) { + for (size_t i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, outputs[i]); + } + } + + /* Trigger all timer's channels in Oneshot mode to fire + * the oneshots with updated values. + */ + if (num_control_groups_updated > 0) { + up_pwm_update(); + } } void -PX4FMU::cycle() +PX4FMU::Run() { - while (true) { - - if (_groups_subscribed != _groups_required) { - subscribe(); - _groups_subscribed = _groups_required; - /* force setting update rate */ - _current_update_rate = 0; - } - - int poll_timeout = 10; - - if (!_run_as_task) { - /* - * Adjust actuator topic update rate to keep up with - * the highest servo update rate configured. - * - * We always mix at max rate; some channels may update slower. - */ - unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; - - if (_current_update_rate != max_rate) { - _current_update_rate = max_rate; - int update_rate_in_ms = int(1000 / _current_update_rate); - - /* reject faster than 500 Hz updates */ - if (update_rate_in_ms < 2) { - update_rate_in_ms = 2; - } - - /* reject slower than 10 Hz updates */ - if (update_rate_in_ms > 100) { - update_rate_in_ms = 100; - } - - PX4_DEBUG("adjusted actuator update interval to %ums", update_rate_in_ms); - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - orb_set_interval(_control_subs[i], update_rate_in_ms); - } - } - - // set to current max rate, even if we are actually checking slower/faster - _current_update_rate = max_rate; - } - - /* check if anything updated */ - poll_timeout = 0; - } - - /* wait for an update */ - unsigned n_updates = 0; - int ret = px4_poll(_poll_fds, _poll_fds_num, poll_timeout); - - /* this would be bad... */ - if (ret < 0) { - PX4_DEBUG("poll error %d", errno); - - } else if (ret == 0) { - /* timeout: no control data, switch to failsafe values */ - // PX4_WARN("no PWM: failsafe"); - - } else { - if (_mixers != nullptr) { - /* get controls for required topics */ - unsigned poll_id = 0; - - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs[i] >= 0) { - - if (_poll_fds[poll_id].revents & POLLIN) { - if (i == 0) { - n_updates++; - } - - orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); - } - - poll_id++; - } - - /* During ESC calibration, we overwrite the throttle value. */ - if (i == 0 && _armed.in_esc_calibration_mode) { + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); - /* Set all controls to 0 */ - memset(&_controls[i], 0, sizeof(_controls[i])); - - /* except thrust to maximum. */ - _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; - - /* Switch off the PWM limit ramp for the calibration. */ - _pwm_limit.state = PWM_LIMIT_STATE_ON; - } - } - } - } // poll_fds - - /* run the mixers on every cycle */ - { - if (_mixers != nullptr) { - - if (_mot_t_max > FLT_EPSILON) { - hrt_abstime now = hrt_absolute_time(); - float dt = (now - _time_last_mix) / 1e6f; - _time_last_mix = now; - - if (dt < 0.0001f) { - dt = 0.0001f; - - } else if (dt > 0.02f) { - dt = 0.02f; - } - - // maximum value the outputs of the multirotor mixer are allowed to change in this cycle - // factor 2 is needed because actuator outputs are in the range [-1,1] - const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max; - _mixers->set_max_delta_out_once(delta_out_max); - } - - _mixers->set_thrust_factor(_thr_mdl_fac); - _mixers->set_airmode(_airmode); - - /* do mixing */ - float outputs[_MAX_ACTUATORS]; - const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs); - - /* the PWM limit call takes care of out of band errors, NaN and constrains */ - uint16_t pwm_limited[_MAX_ACTUATORS]; - - pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); - - /* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */ - if (_armed.force_failsafe) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - pwm_limited[i] = _failsafe_pwm[i]; - } - } - - /* overwrite outputs in case of lockdown or parachute triggering with disarmed PWM values */ - if (_armed.lockdown || _armed.manual_lockdown) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - pwm_limited[i] = _disarmed_pwm[i]; - } - } - - /* apply _motor_ordering */ - reorder_outputs(pwm_limited); - - /* output to the servos */ - if (_pwm_initialized && !_test_mode) { - for (size_t i = 0; i < mixed_num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - } - - /* Trigger all timer's channels in Oneshot mode to fire - * the oneshots with updated values. - */ - if (n_updates > 0 && !_test_mode) { - up_pwm_update(); - } - - actuator_outputs_s actuator_outputs = {}; - actuator_outputs.timestamp = hrt_absolute_time(); - actuator_outputs.noutputs = mixed_num_outputs; - - // zero unused outputs - for (size_t i = 0; i < mixed_num_outputs; ++i) { - actuator_outputs.output[i] = pwm_limited[i]; - } - - orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &actuator_outputs, &_class_instance, ORB_PRIO_DEFAULT); - - /* publish mixer status */ - MultirotorMixer::saturation_status saturation_status; - saturation_status.value = _mixers->get_saturation_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; - - orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance, ORB_PRIO_DEFAULT); - } - - // use first valid timestamp_sample for latency tracking - for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - const bool required = _groups_required & (1 << i); - const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; - - if (required && (timestamp_sample > 0)) { - perf_set_elapsed(_perf_control_latency, actuator_outputs.timestamp - timestamp_sample); - break; - } - } - } - } - - _cycle_timestamp = hrt_absolute_time(); - -#ifdef GPIO_BTN_SAFETY - - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - - if (_cycle_timestamp - _last_safety_check >= (unsigned int)1e5) { - _last_safety_check = _cycle_timestamp; - - /** - * Get and handle the safety status at 10Hz - */ - struct safety_s safety = {}; - - if (!_safety_disabled) { - /* read safety switch input and control safety switch LED at 10Hz */ - safety_check_button(); - } - - /* Make the safety button flash anyway, no matter if it's used or not. */ - flash_safety_button(); - - safety.timestamp = hrt_absolute_time(); - safety.safety_switch_available = true; - safety.safety_off = _safety_btn_off; - - /* lazily publish the safety status */ - if (_to_safety != nullptr) { - orb_publish(ORB_ID(safety), _to_safety, &safety); - - } else { - int instance; - _to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT); - } - } - } - -#endif - - /* check safety button state */ - bool updated = false; - orb_check(_safety_sub, &updated); - - if (updated) { - safety_s safety; - - if (orb_copy(ORB_ID(safety), _safety_sub, &safety) == 0) { - _safety_off = !safety.safety_switch_available || safety.safety_off; - } - } - - /* check arming state */ - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* Update the armed status and check that we're not locked down. - * We also need to arm throttle for the ESC calibration. */ - _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || - (_safety_off && _armed.in_esc_calibration_mode); - } - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = _armed.armed || _num_disarmed_set > 0 || _armed.in_esc_calibration_mode; + exit_and_cleanup(); + return; + } - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; + perf_begin(_cycle_perf); + perf_count(_cycle_interval_perf); - update_pwm_out_state(pwm_on); - } + _mixing_output.update(); - orb_check(_param_sub, &updated); + /* 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; - if (updated) { - this->update_params(); - } + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + update_pwm_out_state(pwm_on); + } - if (_run_as_task) { - if (should_exit()) { - break; - } + if (_param_sub.updated()) { + update_params(); + } - } else { - if (should_exit()) { - exit_and_cleanup(); + if (_current_update_rate == 0) { + update_current_rate(); + } - } else { - /* schedule next cycle */ - work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL)); - } + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true); - break; - } - } + perf_end(_cycle_perf); } void PX4FMU::update_params() { parameter_update_s pupdate; - orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); + _param_sub.update(&pupdate); update_pwm_rev_mask(); update_pwm_trims(); - param_t param_handle; - - // maximum motor slew rate parameter - param_handle = param_find("MOT_SLEW_MAX"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_mot_t_max); - } - - // thrust to pwm modelling factor - param_handle = param_find("THR_MDL_FAC"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_thr_mdl_fac); - } - - // multicopter air-mode - param_handle = param_find("MC_AIRMODE"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, &_airmode); - } - - // motor ordering - param_handle = param_find("MOT_ORDERING"); - - if (param_handle != PARAM_INVALID) { - param_get(param_handle, (int32_t *)&_motor_ordering); - } + updateParams(); } -int -PX4FMU::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls[control_group].control[control_index]; - - /* limit control input */ - if (input > 1.0f) { - input = 1.0f; - - } else if (input < -1.0f) { - input = -1.0f; - } - - /* motor spinup phase - lock throttle to zero */ - if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* limit the throttle output to zero during motor spinup, - * as the motors cannot follow any demand yet - */ - input = 0.0f; - } - } - - /* throttle not arming - mark throttle input as invalid */ - if (arm_nothrottle() && !_armed.in_esc_calibration_mode) { - if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || - control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && - control_index == actuator_controls_s::INDEX_THROTTLE) { - /* set the throttle to an invalid value */ - input = NAN; - } - } - - return 0; -} - int PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { @@ -1502,16 +858,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: - break; - case PWM_SERVO_SET_FORCE_SAFETY_OFF: - /* force safety switch off */ - _safety_btn_off = true; - break; - case PWM_SERVO_SET_FORCE_SAFETY_ON: - /* force safety switch on */ - _safety_btn_off = false; break; case PWM_SERVO_DISARM: @@ -1547,7 +895,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1556,33 +904,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _failsafe_pwm[i] = PWM_HIGHEST_MAX; + _mixing_output.failsafeValue(i) = PWM_HIGHEST_MAX; } #if PWM_LOWEST_MIN > 0 else if (pwm->values[i] < PWM_LOWEST_MIN) { - _failsafe_pwm[i] = PWM_LOWEST_MIN; + _mixing_output.failsafeValue(i) = PWM_LOWEST_MIN; } #endif else { - _failsafe_pwm[i] = pwm->values[i]; - } - } - - /* - * update the counter - * this is needed to decide if disarmed PWM output should be turned on or not - */ - _num_failsafe_set = 0; - - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - if (_failsafe_pwm[i] > 0) { - _num_failsafe_set++; + _mixing_output.failsafeValue(i) = pwm->values[i]; } } @@ -1592,11 +928,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_FAILSAFE_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - pwm->values[i] = _failsafe_pwm[i]; + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.failsafeValue(i); } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; break; } @@ -1604,7 +940,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1613,19 +949,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _disarmed_pwm[i] = PWM_HIGHEST_MAX; + _mixing_output.disarmedValue(i) = PWM_HIGHEST_MAX; } #if PWM_LOWEST_MIN > 0 else if (pwm->values[i] < PWM_LOWEST_MIN) { - _disarmed_pwm[i] = PWM_LOWEST_MIN; + _mixing_output.disarmedValue(i) = PWM_LOWEST_MIN; } #endif else { - _disarmed_pwm[i] = pwm->values[i]; + _mixing_output.disarmedValue(i) = pwm->values[i]; } } @@ -1635,8 +971,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) */ _num_disarmed_set = 0; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - if (_disarmed_pwm[i] > 0) { + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + if (_mixing_output.disarmedValue(i) > 0) { _num_disarmed_set++; } } @@ -1647,11 +983,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_DISARMED_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - pwm->values[i] = _disarmed_pwm[i]; + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.disarmedValue(i); } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; break; } @@ -1659,7 +995,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1668,20 +1004,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] > PWM_HIGHEST_MIN) { - _min_pwm[i] = PWM_HIGHEST_MIN; + _mixing_output.minValue(i) = PWM_HIGHEST_MIN; } #if PWM_LOWEST_MIN > 0 else if (pwm->values[i] < PWM_LOWEST_MIN) { - _min_pwm[i] = PWM_LOWEST_MIN; + _mixing_output.minValue(i) = PWM_LOWEST_MIN; } #endif else { - _min_pwm[i] = pwm->values[i]; + _mixing_output.minValue(i) = pwm->values[i]; } } @@ -1691,11 +1027,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_MIN_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - pwm->values[i] = _min_pwm[i]; + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.minValue(i); } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; arg = (unsigned long)&pwm; break; } @@ -1704,7 +1040,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { ret = -EINVAL; break; } @@ -1713,13 +1049,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (pwm->values[i] == 0) { /* ignore 0 */ } else if (pwm->values[i] < PWM_LOWEST_MAX) { - _max_pwm[i] = PWM_LOWEST_MAX; + _mixing_output.maxValue(i) = PWM_LOWEST_MAX; } else if (pwm->values[i] > PWM_HIGHEST_MAX) { - _max_pwm[i] = PWM_HIGHEST_MAX; + _mixing_output.maxValue(i) = PWM_HIGHEST_MAX; } else { - _max_pwm[i] = pwm->values[i]; + _mixing_output.maxValue(i) = pwm->values[i]; } } @@ -1729,11 +1065,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_MAX_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - for (unsigned i = 0; i < _MAX_ACTUATORS; i++) { - pwm->values[i] = _max_pwm[i]; + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + pwm->values[i] = _mixing_output.maxValue(i); } - pwm->channel_count = _MAX_ACTUATORS; + pwm->channel_count = MAX_ACTUATORS; arg = (unsigned long)&pwm; break; } @@ -1742,20 +1078,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) struct pwm_output_values *pwm = (struct pwm_output_values *)arg; /* discard if too many values are sent */ - if (pwm->channel_count > _MAX_ACTUATORS) { + if (pwm->channel_count > MAX_ACTUATORS) { PX4_DEBUG("error: too many trim values: %d", pwm->channel_count); ret = -EINVAL; break; } - if (_mixers == nullptr) { + if (_mixing_output.mixers() == nullptr) { PX4_ERR("error: no mixer loaded"); ret = -EIO; break; } /* copy the trim values to the mixer offsets */ - _mixers->set_trims((int16_t *)pwm->values, pwm->channel_count); + _mixing_output.mixers()->set_trims((int16_t *)pwm->values, pwm->channel_count); PX4_DEBUG("set_trims: %d, %d, %d, %d", pwm->values[0], pwm->values[1], pwm->values[2], pwm->values[3]); break; @@ -1764,13 +1100,13 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_TRIM_PWM: { struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - if (_mixers == nullptr) { + 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 = _mixers->get_trims((int16_t *)pwm->values); + pwm->channel_count = _mixing_output.mixers()->get_trims((int16_t *)pwm->values); } break; @@ -1882,6 +1218,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + /* FALLTHROUGH */ case PWM_SERVO_GET(4): if (_mode < MODE_5PWM1CAP) { ret = -EINVAL; @@ -2125,44 +1462,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) } case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - } + _mixing_output.resetMixerThreadSafe(); break; case MIXERIOCLOADBUF: { const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - - if (_mixers == nullptr) { - _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); - } - - if (_mixers == nullptr) { - _groups_required = 0; - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(buf, buflen); - - if (ret != 0) { - PX4_DEBUG("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - _groups_required = 0; - ret = -EINVAL; - - } else { - - _mixers->groups_required(_groups_required); - PX4_DEBUG("loaded mixers \n%s\n", buf); - update_pwm_trims(); - } - } + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + update_pwm_trims(); break; } @@ -2177,75 +1485,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } -/* - this implements PWM output via a write() method, for compatibility - with px4io - */ -ssize_t -PX4FMU::write(file *filp, const char *buffer, size_t len) -{ - unsigned count = len / 2; - uint16_t values[_MAX_ACTUATORS]; - -#if BOARD_HAS_PWM == 0 - return 0; -#endif - - if (count > BOARD_HAS_PWM) { - // we have at most BOARD_HAS_PWM outputs - count = BOARD_HAS_PWM; - } - - if (count > _MAX_ACTUATORS) { - count = _MAX_ACTUATORS; - } - - // allow for misaligned values - memcpy(values, buffer, count * 2); - - for (unsigned i = count; i < _num_outputs; ++i) { - values[i] = PWM_IGNORE_THIS_CHANNEL; - } - - reorder_outputs(values); - - for (unsigned i = 0; i < _num_outputs; i++) { - if (values[i] != PWM_IGNORE_THIS_CHANNEL) { - up_pwm_servo_set(i, values[i]); - } - } - - return count * 2; -} - -void -PX4FMU::reorder_outputs(uint16_t values[_MAX_ACTUATORS]) -{ - if (_MAX_ACTUATORS < 4) { - return; - } - - if (_motor_ordering == MotorOrdering::Betaflight) { - /* - * Betaflight default motor ordering: - * 4 2 - * ^ - * 3 1 - */ - const uint16_t pwm_tmp[4] = {values[0], values[1], values[2], values[3] }; - values[0] = pwm_tmp[3]; - values[1] = pwm_tmp[0]; - values[2] = pwm_tmp[1]; - values[3] = pwm_tmp[2]; - } - - /* else: PX4, no need to reorder - * 3 1 - * ^ - * 2 4 - */ -} - void PX4FMU::sensor_reset(int ms) { @@ -2646,21 +1885,9 @@ PX4FMU::test() servos[i] = pwm_value; } - if (direction == 1) { - // use ioctl interface for one direction - 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; - } - } - - } else { - // and use write interface for the other direction - ret = ::write(fd, servos, sizeof(servos)); - - if (ret != (int)sizeof(servos)) { - PX4_ERR("error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + 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; } } @@ -2805,12 +2032,6 @@ PX4FMU::fake(int argc, char *argv[]) return 0; } -PX4FMU *PX4FMU::instantiate(int argc, char *argv[]) -{ - // No arguments to parse. We also know that we should run as task - return new PX4FMU(true); -} - int PX4FMU::custom_command(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; @@ -2975,11 +2196,7 @@ driver. Alternatively, the fmu can be started in one of the capture modes, and t callback with ioctl calls. ### Implementation -By default the module runs on the work queue, to reduce RAM usage. It can also be run in its own thread, -specified via start flag -t, to reduce latency. -When running on the work queue, it schedules at a fixed frequency, and the pwm rate limits the update rate of -the actuator_controls topics. In case of running in its own thread, the module polls on the actuator_controls topic. -Additionally the pwm rate defines the lower-level IO timer rates. +By default the module runs on a work queue with a callback on the uORB actuator_controls topic. ### Examples It is typically started with: @@ -2997,7 +2214,6 @@ mixer files. PRINT_MODULE_USAGE_NAME("fmu", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); - PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true); PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the fmu if not running already"); @@ -3040,17 +2256,7 @@ mixer files. int PX4FMU::print_status() { - PX4_INFO("Running %s", (_run_as_task ? "as task" : "on work queue")); - - if (!_run_as_task) { - PX4_INFO("Max update rate: %i Hz", _current_update_rate); - } - -#ifdef GPIO_BTN_SAFETY - if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on"); - } -#endif + PX4_INFO("Max update rate: %i Hz", _current_update_rate); const char *mode_str = nullptr; @@ -3096,6 +2302,10 @@ int PX4FMU::print_status() PX4_INFO("PWM Mode: %s", mode_str); } + perf_print_counter(_cycle_perf); + perf_print_counter(_cycle_interval_perf); + _mixing_output.printStatus(); + return 0; } diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index df4ed00bda44..30f2867c5afa 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-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 @@ -50,19 +50,3 @@ * @group PWM Outputs */ PARAM_DEFINE_INT32(MOT_ORDERING, 0); - -/** - * Run the FMU as a task to reduce latency - * - * If true, the FMU will run in a separate task instead of on the work queue. - * Set this if low latency is required, for example for racing. - * - * This is a trade-off between RAM usage and latency: running as a task, it - * requires a separate stack and directly polls on the control topics, whereas - * running on the work queue, it runs at a fixed update rate. - * - * @boolean - * @reboot_required true - * @group System - */ -PARAM_DEFINE_INT32(SYS_FMU_TASK, 1); diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index 1cf4809fc6b6..f82c3f9487e6 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -33,16 +33,14 @@ px4_add_module( MODULE drivers__px4io MAIN px4io - STACK_MAIN 1816 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS px4io.cpp - px4io_uploader.cpp px4io_serial.cpp - px4io_serial_f4.cpp - px4io_serial_f7.cpp + px4io_uploader.cpp DEPENDS + arch_px4io_serial circuit_breaker mixer ) @@ -59,6 +57,7 @@ ExternalProject_Add(px4io_firmware INSTALL_COMMAND "" USES_TERMINAL_BUILD true DEPENDS git_nuttx git_nuttx_apps + BUILD_ALWAYS 1 ) ExternalProject_Get_Property(px4io_firmware BINARY_DIR) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 21c5abab43bd..e61f291f58b7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -76,6 +76,7 @@ #include #include +#include #include #include #include @@ -738,7 +739,8 @@ PX4IO::init() vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION; /* send command once */ - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); /* spin here until IO's state has propagated into the system */ do { @@ -755,7 +757,7 @@ PX4IO::init() /* re-send if necessary */ if (!actuator_armed.force_failsafe) { - orb_publish(ORB_ID(vehicle_command), pub, &vcmd); + vcmd_pub.publish(vcmd); PX4_WARN("re-sending flight termination cmd"); } @@ -769,7 +771,8 @@ PX4IO::init() vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM; /* send command once */ - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); /* spin here until IO's state has propagated into the system */ do { @@ -786,7 +789,7 @@ PX4IO::init() /* re-send if necessary */ if (!actuator_armed.armed) { - orb_publish(ORB_ID(vehicle_command), pub, &vcmd); + vcmd_pub.publish(vcmd); PX4_WARN("re-sending arm cmd"); } @@ -1302,7 +1305,14 @@ PX4IO::io_set_control_state(unsigned group) /* ensure FLOAT_TO_REG does not produce an integer overflow */ const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f); - regs[i] = FLOAT_TO_REG(ctrl); + if (!isfinite(ctrl)) { + regs[i] = INT16_MAX; + + } else { + regs[i] = FLOAT_TO_REG(ctrl); + } + + } if (!_test_fmu_fail) { @@ -1334,6 +1344,13 @@ PX4IO::io_set_arming_state() _armed = armed.armed; + if (armed.prearmed) { + set |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; + + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_PREARMED; + } + if ((armed.lockdown || armed.manual_lockdown) && !_lockdown_override) { set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; _lockdown_override = true; @@ -2274,6 +2291,7 @@ PX4IO::print_status(bool extended_status) printf("arming 0x%04hx%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) ? " FMU_PREARMED" : " FMU_NOT_PREARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index e3808c80e887..a46478b929a0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -39,14 +39,14 @@ #include "px4io_driver.h" -#include "px4io_serial.h" +#include static PX4IO_serial *g_interface; device::Device *PX4IO_serial_interface() { - return new PX4IO_INTERFACE_CLASS(); + return new ArchPX4IOSerial(); } PX4IO_serial::PX4IO_serial() : diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp index d111db42e05d..34da78707332 100644 --- a/src/drivers/qshell/posix/qshell.cpp +++ b/src/drivers/qshell/posix/qshell.cpp @@ -101,7 +101,7 @@ int QShell::_send_cmd(std::vector &argList) qshell_req.strlen = cmd.size(); strcpy((char *)qshell_req.cmd, cmd.c_str()); - qshell_req.sequence = _current_sequence; + qshell_req.request_sequence = _current_sequence; int instance; orb_publish_auto(ORB_ID(qshell_req), &_pub_qshell_req, &qshell_req, &instance, ORB_PRIO_DEFAULT); @@ -131,7 +131,7 @@ int QShell::_wait_for_retval() struct qshell_retval_s retval; orb_copy(ORB_ID(qshell_retval), _sub_qshell_retval, &retval); - if (retval.sequence != _current_sequence) { + if (retval.return_sequence != _current_sequence) { PX4_WARN("Ignoring return value with wrong sequence"); } else { diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp index d159be1bc811..5988fcd07337 100644 --- a/src/drivers/qshell/qurt/qshell.cpp +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include @@ -117,7 +116,7 @@ int QShell::main() struct qshell_retval_s retval; retval.return_value = run_cmd(appargs); - retval.sequence = m_qshell_req.sequence; + retval.return_sequence = m_qshell_req.request_sequence; if (retval.return_value) { PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd); diff --git a/src/drivers/qshell/qurt/qshell_main.cpp b/src/drivers/qshell/qurt/qshell_main.cpp index 95da6bbbb764..ecf966f1aba8 100644 --- a/src/drivers/qshell/qurt/qshell_main.cpp +++ b/src/drivers/qshell/qurt/qshell_main.cpp @@ -38,9 +38,9 @@ * @author Nicolas de Palezieux */ -#include #include #include +#include #include "qshell.h" extern "C" __EXPORT int qshell_entry(int argc, char **argv); diff --git a/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt b/src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt similarity index 99% rename from src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt rename to src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt index 60b553491f31..15562a382ef5 100644 --- a/src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt +++ b/src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt @@ -47,7 +47,6 @@ set_target_properties(mpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_co px4_add_module( MODULE platforms__qurt__fc_addon__mpu_spi MAIN mpu9x50 - STACK_MAIN 1200 INCLUDES ${FC_ADDON}/flight_controller/hexagon/inc SRCS diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp similarity index 100% rename from src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp rename to src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c b/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c similarity index 100% rename from src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c rename to src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c diff --git a/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt b/src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt similarity index 99% rename from src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt rename to src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt index 3ef6cec84a1e..ac5344653b18 100644 --- a/src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt +++ b/src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt @@ -47,7 +47,6 @@ set_target_properties(rc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/fligh px4_add_module( MODULE platforms__qurt__fc_addon__rc_receiver MAIN rc_receiver - STACK_MAIN 1200 INCLUDES ${FC_ADDON}/flight_controller/hexagon/inc SRCS diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp b/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp similarity index 100% rename from src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp rename to src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp diff --git a/src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c b/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c similarity index 100% rename from src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c rename to src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c diff --git a/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt b/src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt similarity index 99% rename from src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt rename to src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt index 969495165524..64f33c77c0e2 100644 --- a/src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt +++ b/src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt @@ -47,7 +47,6 @@ set_target_properties(uart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_c px4_add_module( MODULE platforms__qurt__fc_addon__uart_esc MAIN uart_esc - STACK_MAIN 1200 INCLUDES ${FC_ADDON}/flight_controller/hexagon/inc SRCS diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp similarity index 100% rename from src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp rename to src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp diff --git a/src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c similarity index 100% rename from src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c rename to src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c diff --git a/src/platforms/qurt/tests/hello/CMakeLists.txt b/src/drivers/qurt/tests/hello/CMakeLists.txt similarity index 100% rename from src/platforms/qurt/tests/hello/CMakeLists.txt rename to src/drivers/qurt/tests/hello/CMakeLists.txt diff --git a/src/platforms/qurt/tests/hello/hello_example.cpp b/src/drivers/qurt/tests/hello/hello_example.cpp similarity index 100% rename from src/platforms/qurt/tests/hello/hello_example.cpp rename to src/drivers/qurt/tests/hello/hello_example.cpp diff --git a/src/platforms/qurt/tests/hello/hello_example.h b/src/drivers/qurt/tests/hello/hello_example.h similarity index 100% rename from src/platforms/qurt/tests/hello/hello_example.h rename to src/drivers/qurt/tests/hello/hello_example.h diff --git a/src/platforms/qurt/tests/hello/hello_main.cpp b/src/drivers/qurt/tests/hello/hello_main.cpp similarity index 98% rename from src/platforms/qurt/tests/hello/hello_main.cpp rename to src/drivers/qurt/tests/hello/hello_main.cpp index 35e72e8b364d..e8ed25bab8d5 100644 --- a/src/platforms/qurt/tests/hello/hello_main.cpp +++ b/src/drivers/qurt/tests/hello/hello_main.cpp @@ -37,9 +37,9 @@ * * @author Mark Charlebois */ -#include #include #include +#include #include "hello_example.h" #include diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/drivers/qurt/tests/hello/hello_start_qurt.cpp similarity index 100% rename from src/platforms/qurt/tests/hello/hello_start_qurt.cpp rename to src/drivers/qurt/tests/hello/hello_start_qurt.cpp diff --git a/src/platforms/qurt/tests/muorb/CMakeLists.txt b/src/drivers/qurt/tests/muorb/CMakeLists.txt similarity index 100% rename from src/platforms/qurt/tests/muorb/CMakeLists.txt rename to src/drivers/qurt/tests/muorb/CMakeLists.txt diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/drivers/qurt/tests/muorb/muorb_test_example.cpp similarity index 100% rename from src/platforms/qurt/tests/muorb/muorb_test_example.cpp rename to src/drivers/qurt/tests/muorb/muorb_test_example.cpp diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/drivers/qurt/tests/muorb/muorb_test_example.h similarity index 100% rename from src/platforms/qurt/tests/muorb/muorb_test_example.h rename to src/drivers/qurt/tests/muorb/muorb_test_example.h diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/drivers/qurt/tests/muorb/muorb_test_main.cpp similarity index 98% rename from src/platforms/qurt/tests/muorb/muorb_test_main.cpp rename to src/drivers/qurt/tests/muorb/muorb_test_main.cpp index c6818a9270fb..fc172f7a909c 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/drivers/qurt/tests/muorb/muorb_test_main.cpp @@ -37,9 +37,9 @@ * * @author Mark Charlebois */ -#include #include #include +#include #include "muorb_test_example.h" extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp similarity index 99% rename from src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp rename to src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp index ba44169ec3a0..0eef795ca34f 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -39,6 +39,7 @@ #include "muorb_test_example.h" #include #include +#include #include #include #include diff --git a/src/drivers/rc_input/CMakeLists.txt b/src/drivers/rc_input/CMakeLists.txt index 87d417381400..1ad8262481c7 100644 --- a/src/drivers/rc_input/CMakeLists.txt +++ b/src/drivers/rc_input/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__rc_input MAIN rc_input - STACK_MAIN 1200 COMPILE_FLAGS SRCS RCInput.cpp diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt index 72d16eec657e..5543f891465d 100644 --- a/src/drivers/roboclaw/CMakeLists.txt +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS roboclaw_main.cpp RoboClaw.cpp - DEPENDS + MODULE_CONFIG + module.yaml ) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d3eda8609ecf..94a9828334e4 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -56,26 +56,44 @@ #include #include #include +#include + +// The RoboClaw has a serial communication timeout of 10ms. +// Add a little extra to account for timing inaccuracy +#define TIMEOUT_US 10500 + +// If a timeout occurs during serial communication, it will immediately try again this many times +#define TIMEOUT_RETRIES 1 + +// If a timeout occurs while disarmed, it will try again this many times. This should be a higher number, +// because stopping when disarmed is pretty important. +#define STOP_RETRIES 10 -uint8_t RoboClaw::checksum_mask = 0x7f; +// Number of bytes returned by the Roboclaw when sending command 78, read both encoders +#define ENCODER_MESSAGE_SIZE 10 -RoboClaw::RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev): - _address(address), - _pulsesPerRev(pulsesPerRev), +// Number of bytes for commands 18 and 19, read speeds. +#define ENCODER_SPEED_MESSAGE_SIZE 7 + +bool RoboClaw::taskShouldExit = false; + +RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): _uart(0), - _controlPoll(), - _actuators(ORB_ID(actuator_controls_0), 20), - _motor1Position(0), - _motor1Speed(0), - _motor1Overflow(0), - _motor2Position(0), - _motor2Speed(0), - _motor2Overflow(0) + _uart_set(), + _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, + _actuatorsSub(-1), + _lastEncoderCount{0, 0}, + _encoderCounts{0, 0}, + _motorSpeeds{0, 0} + { - // setup control polling - _controlPoll.fd = _actuators.getHandle(); - _controlPoll.events = POLLIN; + _param_handles.actuator_write_period_ms = param_find("RBCLW_WRITE_PER"); + _param_handles.encoder_read_period_ms = param_find("RBCLW_READ_PER"); + _param_handles.counts_per_rev = param_find("RBCLW_COUNTS_REV"); + _param_handles.serial_baud_rate = param_find(baudRateParam); + _param_handles.address = param_find("RBCLW_ADDRESS"); + + _parameters_update(); // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); @@ -83,17 +101,17 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, if (_uart < 0) { err(1, "could not open %s", deviceName); } int ret = 0; - struct termios uart_config; + struct termios uart_config {}; ret = tcgetattr(_uart, &uart_config); if (ret < 0) { err(1, "failed to get attr"); } uart_config.c_oflag &= ~ONLCR; // no CR for every LF - ret = cfsetispeed(&uart_config, B38400); + ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate); if (ret < 0) { err(1, "failed to set input speed"); } - ret = cfsetospeed(&uart_config, B38400); + ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate); if (ret < 0) { err(1, "failed to set output speed"); } @@ -101,6 +119,8 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, if (ret < 0) { err(1, "failed to set attr"); } + FD_ZERO(&_uart_set); + // setup default settings, reset encoders resetEncoders(); } @@ -112,84 +132,196 @@ RoboClaw::~RoboClaw() close(_uart); } -int RoboClaw::readEncoder(e_motor motor) +void RoboClaw::taskMain() { - uint16_t sum = 0; - - if (motor == MOTOR_1) { - _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. + uint8_t rbuff[4]; + int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); - } else if (motor == MOTOR_2) { - _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + if (err_code <= 0) { + PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); + return; } - uint8_t rbuf[50]; - usleep(5000); - int nread = read(_uart, rbuf, 50); + // This main loop performs two different tasks, asynchronously: + // - Send actuator_controls_0 to the Roboclaw as soon as they are available + // - Read the encoder values at a constant rate + // To do this, the timeout on the poll() function is used. + // waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders. + // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before + // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return + // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) + uint64_t encoderTaskLastRun = 0; + int waitTime = 0; - if (nread < 6) { - printf("failed to read\n"); - return -1; - } + uint64_t actuatorsLastWritten = 0; - //printf("received: \n"); - //for (int i=0;i 2000 * _parameters.actuator_write_period_ms; + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); + _parameters_update(); + } + + // No timeout, update on either the actuator controls or the armed state + if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) { + orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); + orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); + + int drive_ret = 0, turn_ret = 0; + + const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown + || _actuatorArmed.force_failsafe || actuators_timeout; + + if (disarmed) { + // If disarmed, I want to be certain that the stop command gets through. + int tries = 0; + + while (tries < STOP_RETRIES && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) { + PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret); + tries++; + px4_usleep(TIMEOUT_US); + } + + } else { + drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]); + turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]); + + if (drive_ret <= 0 || turn_ret <= 0) { + PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); + } + } - int overFlow = 0; + actuatorsLastWritten = hrt_absolute_time(); - if (status & STATUS_REVERSE) { - //printf("roboclaw: reverse\n"); + } else { + // A timeout occurred, which means that it's time to update the encoders + encoderTaskLastRun = hrt_absolute_time(); + + if (readEncoder() > 0) { + + for (int i = 0; i < 2; i++) { + _wheelEncoderMsg[i].timestamp = encoderTaskLastRun; + + _wheelEncoderMsg[i].encoder_position = _encoderCounts[i]; + _wheelEncoderMsg[i].speed = _motorSpeeds[i]; + + if (_wheelEncodersAdv[i] == nullptr) { + int instance; + _wheelEncodersAdv[i] = orb_advertise_multi(ORB_ID(wheel_encoders), &_wheelEncoderMsg[i], + &instance, ORB_PRIO_DEFAULT); + + } else { + orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv[i], &_wheelEncoderMsg[i]); + } + } + + } else { + PX4_ERR("Error reading encoders"); + } + } + + waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun); + waitTime = waitTime < 0 ? 0 : waitTime; } - if (status & STATUS_UNDERFLOW) { - //printf("roboclaw: underflow\n"); - overFlow = -1; + orb_unsubscribe(_actuatorsSub); + orb_unsubscribe(_armedSub); + orb_unsubscribe(_paramSub); +} + +int RoboClaw::readEncoder() +{ - } else if (status & STATUS_OVERFLOW) { - //printf("roboclaw: overflow\n"); - overFlow = +1; + uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE]; + // I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like: + // [ ] + // And I just ignore all of the statuses and checksums. (The _transaction() function internally handles the + // checksum) + uint8_t rbuff_speed[ENCODER_SPEED_MESSAGE_SIZE + 4]; + + bool success = false; + + for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { + success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, + true) == ENCODER_MESSAGE_SIZE; + success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; + success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; } - static int64_t overflowAmount = 0x100000000LL; + if (!success) { + PX4_ERR("Error reading encoders"); + return -1; + } - if (motor == MOTOR_1) { - _motor1Overflow += overFlow; - _motor1Position = float(int64_t(count) + - _motor1Overflow * overflowAmount) / _pulsesPerRev; + uint32_t count; + uint32_t speed; + uint8_t *count_bytes; + uint8_t *speed_bytes; + + for (int motor = 0; motor <= 1; motor++) { + count = 0; + speed = 0; + count_bytes = &rbuff_pos[motor * 4]; + speed_bytes = &rbuff_speed[motor * 4]; + + // Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the + // endianness of the system this code is running on. + for (int byte = 0; byte < 4; byte++) { + count = (count << 8) + count_bytes[byte]; + speed = (speed << 8) + speed_bytes[byte]; + } - } else if (motor == MOTOR_2) { - _motor2Overflow += overFlow; - _motor2Position = float(int64_t(count) + - _motor2Overflow * overflowAmount) / _pulsesPerRev; + // The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting + // at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem + // to work. This code detects overflow manually. + // These diffs are the difference between the count I just read from the Roboclaw and the last + // count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward, + // and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close + // to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32. + // To detect and account for overflow, I just assume that the smaller diff is correct. + // Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this + // will be wrong. But that's 1.7 million revolutions, so it probably won't come up. + uint32_t fwd_diff = count - _lastEncoderCount[motor]; + uint32_t rev_diff = _lastEncoderCount[motor] - count; + // At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe. + int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff); + _encoderCounts[motor] += diff; + _lastEncoderCount[motor] = count; + + _motorSpeeds[motor] = speed; } - return 0; + return 1; } void RoboClaw::printStatus(char *string, size_t n) { - snprintf(string, n, - "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", double(getMotorPosition(MOTOR_1)), double(getMotorSpeed(MOTOR_1)), double(getMotorPosition(MOTOR_2)), @@ -199,10 +331,10 @@ void RoboClaw::printStatus(char *string, size_t n) float RoboClaw::getMotorPosition(e_motor motor) { if (motor == MOTOR_1) { - return _motor1Position; + return _encoderCounts[0]; } else if (motor == MOTOR_2) { - return _motor2Position; + return _encoderCounts[1]; } else { warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); @@ -213,10 +345,10 @@ float RoboClaw::getMotorPosition(e_motor motor) float RoboClaw::getMotorSpeed(e_motor motor) { if (motor == MOTOR_1) { - return _motor1Speed; + return _motorSpeeds[0]; } else if (motor == MOTOR_2) { - return _motor2Speed; + return _motorSpeeds[1]; } else { warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); @@ -226,154 +358,262 @@ float RoboClaw::getMotorSpeed(e_motor motor) int RoboClaw::setMotorSpeed(e_motor motor, float value) { - uint16_t sum = 0; - - // bound - if (value > 1) { value = 1; } - - if (value < -1) { value = -1; } - - uint8_t speed = fabs(value) * 127; + e_command command; // send command if (motor == MOTOR_1) { if (value > 0) { - return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + command = CMD_DRIVE_FWD_1; } else { - return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + command = CMD_DRIVE_REV_1; } } else if (motor == MOTOR_2) { if (value > 0) { - return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + command = CMD_DRIVE_FWD_2; } else { - return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + command = CMD_DRIVE_REV_2; } + + } else { + return -1; } - return -1; + return _sendUnsigned7Bit(command, value); } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) { - uint16_t sum = 0; - - // bound - if (value > 1) { value = 1; } - if (value < -1) { value = -1; } - - int16_t duty = 1500 * value; + e_command command; // send command if (motor == MOTOR_1) { - return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, - (uint8_t *)(&duty), 2, sum); + command = CMD_SIGNED_DUTYCYCLE_1; } else if (motor == MOTOR_2) { - return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, - (uint8_t *)(&duty), 2, sum); + command = CMD_SIGNED_DUTYCYCLE_2; + + } else { + return -1; } - return -1; + return _sendSigned16Bit(command, value); +} + +int RoboClaw::drive(float value) +{ + e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; + return _sendUnsigned7Bit(command, value); +} + +int RoboClaw::turn(float value) +{ + e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; + return _sendUnsigned7Bit(command, value); } int RoboClaw::resetEncoders() { - uint16_t sum = 0; - return _sendCommand(CMD_RESET_ENCODERS, - nullptr, 0, sum); + return _sendNothing(CMD_RESET_ENCODERS); } -int RoboClaw::update() +int RoboClaw::_sendUnsigned7Bit(e_command command, float data) { - // wait for an actuator publication, - // check for exit condition every second - // note "::poll" is required to distinguish global - // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error - - // if new data, send to motors - if (_actuators.updated()) { - _actuators.update(); - setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]); - setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]); + data = fabs(data); + + if (data > 1.0f) { + data = 1.0f; } - return 0; + auto byte = (uint8_t)(data * INT8_MAX); + uint8_t recv_byte; + return _transaction(command, &byte, 1, &recv_byte, 1); } -uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n) +int RoboClaw::_sendSigned16Bit(e_command command, float data) { - uint16_t sum = 0; + if (data > 1.0f) { + data = 1.0f; - //printf("sum\n"); - for (size_t i = 0; i < n; i++) { - sum += buf[i]; - //printf("%d\t", buf[i]); + } else if (data < -1.0f) { + data = -1.0f; } - //printf("total sum %d\n", sum); - return sum; + auto buff = (uint16_t)(data * INT16_MAX); + uint8_t recv_buff; + return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1); } -int RoboClaw::_sendCommand(e_command cmd, uint8_t *data, - size_t n_data, uint16_t &prev_sum) +int RoboClaw::_sendNothing(e_command command) { - tcflush(_uart, TCIOFLUSH); // flush buffers - uint8_t buf[n_data + 3]; - buf[0] = _address; - buf[1] = cmd; + uint8_t recv_buff; + return _transaction(command, nullptr, 0, &recv_buff, 1); +} + +uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) +{ + uint16_t crc = init; + + for (size_t byte = 0; byte < n; byte++) { + crc = crc ^ (((uint16_t) buf[byte]) << 8); - for (size_t i = 0; i < n_data; i++) { - buf[i + 2] = data[n_data - i - 1]; // MSB + for (uint8_t bit = 0; bit < 8; bit++) { + if (crc & 0x8000) { + crc = (crc << 1) ^ 0x1021; + + } else { + crc = crc << 1; + } + } } - uint16_t sum = _sumBytes(buf, n_data + 2); - prev_sum += sum; - buf[n_data + 2] = sum & checksum_mask; - //printf("\nmessage:\n"); - //for (size_t i=0;i> 8) & 0xFF; + buf[wbytes - 1] = sum & 0xFFu; } - roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3); - roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3); + int count = write(_uart, buf, wbytes); - for (int i = 0; i < 10; i++) { - usleep(100000); - roboclaw.readEncoder(RoboClaw::MOTOR_1); - roboclaw.readEncoder(RoboClaw::MOTOR_2); - roboclaw.printStatus(buf, 200); - printf("%s", buf); + if (count < (int) wbytes) { // Did not successfully send all bytes. + PX4_ERR("Only wrote %d out of %d bytes", count, (int) wbytes); + return -1; } - printf("Test complete\n"); - return 0; + // READ + + FD_ZERO(&_uart_set); + FD_SET(_uart, &_uart_set); + + uint8_t *rbuff_curr = rbuff; + size_t bytes_read = 0; + + // select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many + // bytes are available. I need to keep reading until I get the number of bytes I expect. + while (bytes_read < rbytes) { + // select(...) may change this timeout struct (because it is not const). So I reset it every time. + _uart_timeout.tv_sec = 0; + _uart_timeout.tv_usec = TIMEOUT_US; + err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + + // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. + if (err_code <= 0) { + return err_code; + } + + err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + + if (err_code <= 0) { + return err_code; + + } else { + bytes_read += err_code; + rbuff_curr += err_code; + } + } + + //TODO: Clean up this mess of IFs and returns + + if (recv_checksum) { + if (bytes_read < 2) { + return -1; + } + + // The checksum sent back by the roboclaw is calculated based on the address and command bytes as well + // as the data returned. + uint16_t checksum_calc = _calcCRC(buf, 2); + checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); + uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; + + if (checksum_calc == checksum_recv) { + return bytes_read; + + } else { + return -10; + } + + } else { + if (bytes_read == 1 && rbuff[0] == 0xFF) { + return 1; + + } else { + return -11; + } + } } -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 +void RoboClaw::_parameters_update() +{ + param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev); + param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms); + param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms); + param_get(_param_handles.address, &_parameters.address); + + if (_actuatorsSub > 0) { + orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); + } + + int baudRate; + param_get(_param_handles.serial_baud_rate, &baudRate); + + switch (baudRate) { + case 2400: + _parameters.serial_baud_rate = B2400; + break; + + case 9600: + _parameters.serial_baud_rate = B9600; + break; + + case 19200: + _parameters.serial_baud_rate = B19200; + break; + + case 38400: + _parameters.serial_baud_rate = B38400; + break; + + case 57600: + _parameters.serial_baud_rate = B57600; + break; + + case 115200: + _parameters.serial_baud_rate = B115200; + break; + + case 230400: + _parameters.serial_baud_rate = B230400; + break; + + case 460800: + _parameters.serial_baud_rate = B460800; + break; + + default: + _parameters.serial_baud_rate = B2400; + } +} diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 8fe4f5337cbc..2ea6937b7bb5 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,9 +45,16 @@ #include #include +#include #include #include +#include +#include +#include #include +#include +#include +#include /** * This is a driver for the RoboClaw motor controller @@ -56,6 +63,9 @@ class RoboClaw { public: + void taskMain(); + static bool taskShouldExit; + /** control channels */ enum e_channel { CH_VOLTAGE_LEFT = 0, @@ -74,11 +84,9 @@ class RoboClaw * serial port e.g. "/dev/ttyS2" * @param address the adddress of the motor * (selectable on roboclaw) - * @param pulsesPerRev # of encoder - * pulses per revolution of wheel + * @param baudRateParam Name of the parameter that holds the baud rate of this serial port */ - RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev); + RoboClaw(const char *deviceName, const char *baudRateParam); /** * deconstructor @@ -101,26 +109,30 @@ class RoboClaw int setMotorSpeed(e_motor motor, float value); /** - * set the duty cycle of a motor, rev/sec + * set the duty cycle of a motor */ int setMotorDutyCycle(e_motor motor, float value); /** - * reset the encoders - * @return status + * Drive both motors. +1 = full forward, -1 = full backward */ - int resetEncoders(); + int drive(float value); /** - * main update loop that updates RoboClaw motor - * dutycycle based on actuator publication + * Turn. +1 = full right, -1 = full left */ - int update(); + int turn(float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); /** * read data from serial */ - int readEncoder(e_motor motor); + int readEncoder(); /** * print status @@ -129,13 +141,6 @@ class RoboClaw private: - // Quadrature status flags - enum e_quadrature_status_flags { - STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ - STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ - STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ - }; - // commands // We just list the commands we want from the manual here. enum e_command { @@ -146,47 +151,94 @@ class RoboClaw CMD_DRIVE_FWD_2 = 4, CMD_DRIVE_REV_2 = 5, + CMD_DRIVE_FWD_MIX = 8, + CMD_DRIVE_REV_MIX = 9, + CMD_TURN_RIGHT = 10, + CMD_TURN_LEFT = 11, + // encoder commands CMD_READ_ENCODER_1 = 16, CMD_READ_ENCODER_2 = 17, + CMD_READ_SPEED_1 = 18, + CMD_READ_SPEED_2 = 19, CMD_RESET_ENCODERS = 20, + CMD_READ_BOTH_ENCODERS = 78, + CMD_READ_BOTH_SPEEDS = 79, // advanced motor control CMD_READ_SPEED_HIRES_1 = 30, CMD_READ_SPEED_HIRES_2 = 31, CMD_SIGNED_DUTYCYCLE_1 = 32, CMD_SIGNED_DUTYCYCLE_2 = 33, - }; - static uint8_t checksum_mask; + CMD_READ_STATUS = 90 + }; - uint16_t _address; - uint16_t _pulsesPerRev; + struct { + speed_t serial_baud_rate; + int32_t counts_per_rev; + int32_t encoder_read_period_ms; + int32_t actuator_write_period_ms; + int32_t address; + } _parameters{}; + + struct { + param_t serial_baud_rate; + param_t counts_per_rev; + param_t encoder_read_period_ms; + param_t actuator_write_period_ms; + param_t address; + } _param_handles{}; int _uart; - - /** poll structure for control packets */ - struct pollfd _controlPoll; + fd_set _uart_set; + struct timeval _uart_timeout; /** actuator controls subscription */ - uORB::SubscriptionPollable _actuators; + int _actuatorsSub{-1}; + actuator_controls_s _actuatorControls; - // private data - float _motor1Position; - float _motor1Speed; - int16_t _motor1Overflow; + int _armedSub{-1}; + actuator_armed_s _actuatorArmed; - float _motor2Position; - float _motor2Speed; - int16_t _motor2Overflow; + int _paramSub{-1}; + parameter_update_s _paramUpdate; - // private methods - uint16_t _sumBytes(uint8_t *buf, size_t n); - int _sendCommand(e_command cmd, uint8_t *data, size_t n_data, uint16_t &prev_sum); -}; + orb_advert_t _wheelEncodersAdv[2] {nullptr, nullptr}; + wheel_encoders_s _wheelEncoderMsg[2]; -// unit testing -int roboclawTest(const char *deviceName, uint8_t address, - uint16_t pulsesPerRev); + uint32_t _lastEncoderCount[2] {0, 0}; + int64_t _encoderCounts[2] {0, 0}; + int32_t _motorSpeeds[2] {0, 0}; -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 + void _parameters_update(); + + static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); + int _sendUnsigned7Bit(e_command command, float data); + int _sendSigned16Bit(e_command command, float data); + int _sendNothing(e_command); + + /** + * Perform a round-trip write and read. + * + * NOTE: This function is not thread-safe. + * + * @param cmd Command to send to the Roboclaw + * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be + * one or two bytes. Can be null iff wbytes == 0. + * @param wbytes Number of bytes to write. Can be 0. + * @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends + * a checksum for this command. + * @param rbytes Maximum number of bytes to read. + * @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw. + * This is an option because some Roboclaw commands expect no checksum. + * @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare + * it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an + * error is returned. + * If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise. + * @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout + * reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned. + */ + int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, + uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); +}; diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml new file mode 100644 index 000000000000..81e5f694dfee --- /dev/null +++ b/src/drivers/roboclaw/module.yaml @@ -0,0 +1,6 @@ +module_name: Roboclaw Driver +serial_config: + - command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM} + port_config_param: + name: RBCLW_SER_CFG + group: Roboclaw \ No newline at end of file diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 2f07efeb157c..76fee29c0b4f 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -34,17 +34,18 @@ /** - * @ file roboclaw_main.cpp + * @file roboclaw_main.cpp * * RoboClaw Motor Driver * * references: - * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf * */ #include #include +#include #include #include #include @@ -56,9 +57,8 @@ #include #include "RoboClaw.hpp" -static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ +px4_task_t deamon_task; /** * Deamon management function. @@ -77,7 +77,49 @@ static void usage(); static void usage() { - PX4_INFO("usage: roboclaw {start|stop|status|test}"); + PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); + + PRINT_MODULE_DESCRIPTION(R"DESCR_STR( +### Description + +This driver communicates over UART with the [Roboclaw motor driver](http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf). +It performs two tasks: + + - Control the motors based on the `actuator_controls_0` UOrb topic. + - Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic + +In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and +your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4, +use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`. + +### Implementation + +The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks: + + 1. Write `actuator_controls_0` messages to the Roboclaw as they become available + 2. Read encoder data from the Roboclaw at a constant, fixed rate. + +Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw +immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`. + +On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails, +the driver terminates immediately. + +### Examples + +The command to start this driver is: + + $ roboclaw start + +`` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`. +`` is te baud rate. + +All available commands are: + + - `$ roboclaw start ` + - `$ roboclaw status` + - `$ roboclaw stop` + )DESCR_STR"); } /** @@ -91,7 +133,7 @@ static void usage() int roboclaw_main(int argc, char *argv[]) { - if (argc < 2) { + if (argc < 4) { usage(); } @@ -100,48 +142,22 @@ int roboclaw_main(int argc, char *argv[]) if (thread_running) { printf("roboclaw already running\n"); /* this is not an error */ - exit(0); + return 0; } - thread_should_exit = false; + RoboClaw::taskShouldExit = false; deamon_task = px4_task_spawn_cmd("roboclaw", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, - 2048, + 2000, roboclaw_thread_main, (char *const *)argv); - exit(0); - - } else if (!strcmp(argv[1], "test")) { - - const char *deviceName = "/dev/ttyS2"; - uint8_t address = 128; - uint16_t pulsesPerRev = 1200; - - if (argc == 2) { - printf("testing with default settings\n"); - - } else if (argc != 4) { - printf("usage: roboclaw test device address pulses_per_rev\n"); - exit(-1); - - } else { - deviceName = argv[2]; - address = strtoul(argv[3], nullptr, 0); - pulsesPerRev = strtoul(argv[4], nullptr, 0); - } - - printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", - deviceName, address, pulsesPerRev); - - roboclawTest(deviceName, address, pulsesPerRev); - thread_should_exit = true; - exit(0); + return 0; } else if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); + RoboClaw::taskShouldExit = true; + return 0; } else if (!strcmp(argv[1], "status")) { @@ -152,11 +168,11 @@ int roboclaw_main(int argc, char *argv[]) printf("\troboclaw app not started\n"); } - exit(0); + return 0; } usage(); - exit(1); + return 1; } int roboclaw_thread_main(int argc, char *argv[]) @@ -167,32 +183,23 @@ int roboclaw_thread_main(int argc, char *argv[]) argc -= 2; argv += 2; - if (argc < 3) { - printf("usage: roboclaw start device address\n"); + if (argc < 2) { + printf("usage: roboclaw start \n"); return -1; } const char *deviceName = argv[1]; - uint8_t address = strtoul(argv[2], nullptr, 0); - uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0); - - printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", - deviceName, address, pulsesPerRev); + const char *baudRate = argv[2]; // start - RoboClaw roboclaw(deviceName, address, pulsesPerRev); + RoboClaw roboclaw(deviceName, baudRate); thread_running = true; - // loop - while (!thread_should_exit) { - roboclaw.update(); - } + roboclaw.taskMain(); // exit printf("[roboclaw] exiting.\n"); thread_running = false; return 0; } - -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c new file mode 100644 index 000000000000..3f3c938c55c0 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file roboclaw_params.c + * + * Parameters defined by the Roboclaw driver. + * + * The Roboclaw will need to be configured to match these parameters. For information about configuring the + * Roboclaw, see http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf + * + * @author Timothy Scott + */ + + +/** + * Uart write period + * + * How long to wait, in Milliseconds, between writing actuator controls over Uart to the Roboclaw + * @unit ms + * @min 1 + * @max 1000 + * @group Roboclaw driver + */ +PARAM_DEFINE_INT32(RBCLW_WRITE_PER, 10); + +/** + * Encoder read period + * + * How long to wait, in Milliseconds, between reading wheel encoder values over Uart from the Roboclaw + * @unit ms + * @min 1 + * @max 1000 + * @group Roboclaw driver + */ +PARAM_DEFINE_INT32(RBCLW_READ_PER, 10); + +/** + * Encoder counts per revolution + * + * Number of encoder counts for one revolution. The roboclaw treats analog encoders (potentiometers) as having 2047 + * counts per rev. The default value of 1200 corresponds to the default configuration of the Aion R1 rover. + * @min 1 + * @group Roboclaw driver + */ +PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200); + +/** + * Address of the Roboclaw + * + * The Roboclaw can be configured to have an address from 0x80 to 0x87, inclusive. It must be configured to match + * this parameter. + * @min 128 + * @max 135 + * @value 128 0x80 + * @value 129 0x81 + * @value 130 0x82 + * @value 131 0x83 + * @value 132 0x84 + * @value 133 0x85 + * @value 134 0x86 + * @value 135 0x87 + * @group Roboclaw driver + */ +PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128); + +/** + * Roboclaw serial baud rate + * + * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate. + * @min 2400 + * @max 460800 + * @value 2400 2400 baud + * @value 9600 9600 baud + * @value 19200 19200 baud + * @value 38400 38400 baud + * @value 57600 57600 baud + * @value 115200 115200 baud + * @value 230400 230400 baud + * @value 460800 460800 baud + * @group Roboclaw driver + * @reboot_required true + */ +PARAM_DEFINE_INT32(RBCLW_BAUD, 2400); diff --git a/src/drivers/rpi_rc_in/CMakeLists.txt b/src/drivers/rpi_rc_in/CMakeLists.txt index c16b2ebc1716..69ff3eb7dcba 100644 --- a/src/drivers/rpi_rc_in/CMakeLists.txt +++ b/src/drivers/rpi_rc_in/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__rpi_rc_in MAIN rpi_rc_in - STACK_MAIN 1200 COMPILE_FLAGS SRCS rpi_rc_in.cpp diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.h b/src/drivers/rpi_rc_in/rpi_rc_in.h index a956b8775d8d..2398cde8423f 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.h +++ b/src/drivers/rpi_rc_in/rpi_rc_in.h @@ -49,7 +49,7 @@ #include #include -#include +#include #include #include @@ -61,7 +61,7 @@ namespace rpi_rc_in { -class RcInput, public px4::ScheduledWorkItem +class RcInput : public px4::ScheduledWorkItem { public: RcInput() : ScheduledWorkItem(px4::wq_configurations::hp_default) {} diff --git a/src/drivers/safety_button/CMakeLists.txt b/src/drivers/safety_button/CMakeLists.txt new file mode 100644 index 000000000000..f1fb9c3e6b0a --- /dev/null +++ b/src/drivers/safety_button/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__safety_button + MAIN safety_button + SRCS + SafetyButton.cpp + DEPENDS + circuit_breaker + px4_work_queue + ) diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp new file mode 100644 index 000000000000..2d1a2a202ed3 --- /dev/null +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -0,0 +1,236 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "SafetyButton.hpp" + +using namespace time_literals; + +static constexpr uint8_t CYCLE_COUNT{10}; /* safety switch must be held for 1 second to activate */ + +// Define the various LED flash sequences for each system state. +enum class LED_PATTERN : uint16_t { + FMU_OK_TO_ARM = 0x0003, /**< slow blinking */ + FMU_REFUSE_TO_ARM = 0x5555, /**< fast blinking */ + IO_ARMED = 0x5050, /**< long off, then double blink */ + FMU_ARMED = 0x5500, /**< long off, then quad blink */ + IO_FMU_ARMED = 0xffff, /**< constantly on */ +}; + +SafetyButton::~SafetyButton() +{ + ScheduleClear(); +} + +void +SafetyButton::CheckButton() +{ + // Debounce the safety button, change state if it has been held for long enough. + bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY); + + /* + * Keep pressed for a while to arm. + * + * Note that the counting sequence has to be same length + * for arming / disarming in order to end up as proper + * state machine, keep ARM_COUNTER_THRESHOLD the same + * length in all cases of the if/else struct below. + */ + if (safety_button_pressed && !_safety_btn_off) { + if (_button_counter < CYCLE_COUNT) { + _button_counter++; + + } else if (_button_counter == CYCLE_COUNT) { + // switch to armed state + _safety_btn_off = true; + _button_counter++; + } + + } else if (safety_button_pressed && _safety_btn_off) { + + if (_button_counter < CYCLE_COUNT) { + _button_counter++; + + } else if (_button_counter == CYCLE_COUNT) { + // change to disarmed state and notify + _safety_btn_off = false; + _button_counter++; + } + + } else { + _button_counter = 0; + } +} + +void +SafetyButton::FlashButton() +{ +#if defined(GPIO_LED_SAFETY) + actuator_armed_s armed; + + if (_armed_sub.copy(&armed)) { + // Select the appropriate LED flash pattern depending on the current arm state + LED_PATTERN pattern = LED_PATTERN::FMU_REFUSE_TO_ARM; + + // cycle the blink state machine at 10Hz + if (_safety_btn_off) { + if (armed.armed) { + pattern = LED_PATTERN::IO_FMU_ARMED; + + } else { + pattern = LED_PATTERN::IO_ARMED; + } + + } else if (armed.armed) { + pattern = LED_PATTERN::FMU_ARMED; + + } else { + pattern = LED_PATTERN::FMU_OK_TO_ARM; + } + + // Turn the LED on if we have a 1 at the current bit position + px4_arch_gpiowrite(GPIO_LED_SAFETY, !((uint16_t)pattern & (1 << _blink_counter++))); + + if (_blink_counter > 15) { + _blink_counter = 0; + } + } + +#endif // GPIO_LED_SAFETY +} + +void +SafetyButton::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + } + + // read safety switch input and control safety switch LED at 10Hz + CheckButton(); + + // Make the safety button flash anyway, no matter if it's used or not. + FlashButton(); + + safety_s safety{}; + safety.timestamp = hrt_absolute_time(); + safety.safety_switch_available = true; + safety.safety_off = _safety_btn_off; + + // publish the safety status + _to_safety.publish(safety); +} + +int +SafetyButton::task_spawn(int argc, char *argv[]) +{ + if (PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { + PX4_ERR("not starting (use px4io for safety button)"); + + return PX4_ERROR; + + } else if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + PX4_WARN("disabled by CBRK_IO_SAFETY, exiting"); + return PX4_ERROR; + + } else { + SafetyButton *instance = new SafetyButton(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->Start() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + } + + return PX4_ERROR; +} + +int +SafetyButton::Start() +{ + ScheduleOnInterval(100_ms); // run at 10 Hz + + return PX4_OK; +} + +int +SafetyButton::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +SafetyButton::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for the safety button. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("safety_button", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the safety button driver"); + + return 0; +} + +int +SafetyButton::print_status() +{ + PX4_INFO("Safety State (from button): %s", _safety_btn_off ? "off" : "on"); + + return 0; +} + +extern "C" __EXPORT int safety_button_main(int argc, char *argv[]); + +int +safety_button_main(int argc, char *argv[]) +{ + return SafetyButton::main(argc, argv); +} diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp new file mode 100644 index 000000000000..0cab2b385fde --- /dev/null +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SafetyButton : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + SafetyButton() : ScheduledWorkItem(px4::wq_configurations::hp_default) {} + virtual ~SafetyButton(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + int Start(); + +private: + + void CheckButton(); + void FlashButton(); + + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Publication _to_safety{ORB_ID(safety)}; + + uint8_t _button_counter{0}; + uint8_t _blink_counter{0}; + bool _safety_off{false}; ///< State of the safety button from the subscribed safety topic + bool _safety_btn_off{false}; ///< State of the safety button read from the HW button + +}; diff --git a/src/drivers/snapdragon_pwm_out/CMakeLists.txt b/src/drivers/snapdragon_pwm_out/CMakeLists.txt index cf0a14f77e67..6aed921c7a5c 100644 --- a/src/drivers/snapdragon_pwm_out/CMakeLists.txt +++ b/src/drivers/snapdragon_pwm_out/CMakeLists.txt @@ -38,5 +38,5 @@ px4_add_module( snapdragon_pwm_out.cpp DEPENDS mixer - pwm_limit + output_limit ) diff --git a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp index 5e38fed10f8d..d642a2c5fdcb 100644 --- a/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp +++ b/src/drivers/snapdragon_pwm_out/snapdragon_pwm_out.cpp @@ -53,7 +53,7 @@ #include #include #include -#include +#include #include /* @@ -105,7 +105,7 @@ px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; uint32_t _groups_required = 0; // limit for pwm -pwm_limit_t _pwm_limit; +output_limit_t _pwm_limit; // esc parameters int32_t _pwm_disarmed; @@ -364,7 +364,7 @@ void task_main(int argc, char *argv[]) _armed.prearmed = false; // set max min pwm - pwm_limit_init(&_pwm_limit); + output_limit_init(&_pwm_limit); _perf_control_latency = perf_alloc(PC_ELAPSED, "snapdragon_pwm_out control latency"); @@ -439,9 +439,9 @@ void task_main(int argc, char *argv[]) // TODO FIXME: pre-armed seems broken -> copied and pasted from pwm_out_rc_in: needs to be tested - pwm_limit_calc(_armed.armed, - false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, - min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); + output_limit_calc(_armed.armed, + false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, disarmed_pwm, + min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); // send and publish outputs if (_armed.lockdown || _armed.manual_lockdown || timeout) { diff --git a/src/drivers/stm32/CMakeLists.txt b/src/drivers/stm32/CMakeLists.txt deleted file mode 100644 index aa444a97dd4f..000000000000 --- a/src/drivers/stm32/CMakeLists.txt +++ /dev/null @@ -1,44 +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_arch - drv_hrt.c - drv_io_timer.c - drv_pwm_servo.c - drv_pwm_trigger.c - drv_input_capture.c - drv_led_pwm.cpp -) -add_dependencies(drivers_arch prebuild_targets) -target_link_libraries(drivers_arch PRIVATE drivers_board) -target_compile_options(drivers_arch PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/src/drivers/stm32/drv_input_capture.h b/src/drivers/stm32/drv_input_capture.h deleted file mode 100644 index 968ce831efaf..000000000000 --- a/src/drivers/stm32/drv_input_capture.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-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 drv_input_capture.h - * - * stm32-specific input capture data. - */ - -#pragma once - -#include diff --git a/src/drivers/tap_esc/CMakeLists.txt b/src/drivers/tap_esc/CMakeLists.txt index fc77f26cea1c..41eb096fb59f 100644 --- a/src/drivers/tap_esc/CMakeLists.txt +++ b/src/drivers/tap_esc/CMakeLists.txt @@ -40,6 +40,5 @@ px4_add_module( tap_esc_common.cpp DEPENDS mixer - pwm_limit ) diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 3a66f3e57b89..c4fee63e43ac 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -60,7 +60,6 @@ #include #include #include -#include #include "tap_esc_common.h" diff --git a/src/drivers/telemetry/bst/CMakeLists.txt b/src/drivers/telemetry/bst/CMakeLists.txt index 1a969fa3c0c0..c9c9e7ff798e 100644 --- a/src/drivers/telemetry/bst/CMakeLists.txt +++ b/src/drivers/telemetry/bst/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__bst MAIN bst - STACK_MAIN 1200 COMPILE_FLAGS SRCS bst.cpp diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 1745975adbc0..9f327ddbbf5b 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt b/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt index 8f2bed90cfa0..719227703b75 100644 --- a/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__frsky_telemetry MAIN frsky_telemetry - STACK_MAIN 1500 COMPILE_FLAGS SRCS frsky_data.cpp diff --git a/src/drivers/telemetry/iridiumsbd/CMakeLists.txt b/src/drivers/telemetry/iridiumsbd/CMakeLists.txt index 5c603f91c26c..7380277a737e 100644 --- a/src/drivers/telemetry/iridiumsbd/CMakeLists.txt +++ b/src/drivers/telemetry/iridiumsbd/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE drivers__iridiumsbd MAIN iridiumsbd STACK 1024 - STACK_MAIN 1024 COMPILE_FLAGS SRCS IridiumSBD.cpp diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 9c118fc5c593..ade79e314ca4 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -1111,14 +1111,8 @@ void IridiumSBD::publish_iridium_status() // publish the status if it changed if (need_to_publish) { - if (_iridiumsbd_status_pub == nullptr) { - _iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status); - - } else { - orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status); - } + _iridiumsbd_status_pub.publish(_status); } - } void IridiumSBD::publish_subsystem_status() @@ -1134,12 +1128,7 @@ void IridiumSBD::publish_subsystem_status() _info.enabled = enabled; _info.ok = ok; - if (_subsystem_pub == nullptr) { - _subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info); - } + _subsystem_pub.publish(_info); } } diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 18085182a7c9..85fae0ab4201 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -39,7 +39,8 @@ #include #include -#include +#include +#include #include #include @@ -304,8 +305,8 @@ class IridiumSBD : public cdev::CDev bool _writing_mavlink_packet = false; uint16_t _packet_length = 0; - orb_advert_t _iridiumsbd_status_pub = nullptr; - orb_advert_t _subsystem_pub = nullptr; + uORB::Publication _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)}; + uORB::PublicationQueued _subsystem_pub{ORB_ID(subsystem_info)}; bool _test_pending = false; char _test_command[32]; diff --git a/src/drivers/test_ppm/CMakeLists.txt b/src/drivers/test_ppm/CMakeLists.txt index bbc85147bcac..dbe3b7b64828 100644 --- a/src/drivers/test_ppm/CMakeLists.txt +++ b/src/drivers/test_ppm/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE drivers__test_ppm MAIN test_ppm - STACK_MAIN 1200 COMPILE_FLAGS SRCS test_ppm.cpp diff --git a/src/drivers/tone_alarm/CMakeLists.txt b/src/drivers/tone_alarm/CMakeLists.txt index f51963a17893..a298671f8bdb 100644 --- a/src/drivers/tone_alarm/CMakeLists.txt +++ b/src/drivers/tone_alarm/CMakeLists.txt @@ -38,6 +38,6 @@ px4_add_module( ToneAlarm.cpp DEPENDS circuit_breaker - tone_alarm_interface + arch_tone_alarm tunes ) diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 86a01360b06d..bd5d12c6a818 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -75,19 +75,10 @@ int ToneAlarm::init() void ToneAlarm::next_note() { if (!_should_run) { - if (_tune_control_sub >= 0) { - orb_unsubscribe(_tune_control_sub); - } - _running = false; return; } - // Subscribe to tune_control. - if (_tune_control_sub < 0) { - _tune_control_sub = orb_subscribe(ORB_ID(tune_control)); - } - // Check for updates orb_update(); @@ -136,11 +127,8 @@ void ToneAlarm::Run() void ToneAlarm::orb_update() { // Check for updates - bool updated = false; - orb_check(_tune_control_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune); + if (_tune_control_sub.updated()) { + _tune_control_sub.copy(&_tune); if (_tune.timestamp > 0) { _play_tone = _tunes.set_control(_tune) == 0; diff --git a/src/drivers/tone_alarm/ToneAlarm.h b/src/drivers/tone_alarm/ToneAlarm.h index 0816ad9ce681..b01d7b58c9b1 100644 --- a/src/drivers/tone_alarm/ToneAlarm.h +++ b/src/drivers/tone_alarm/ToneAlarm.h @@ -43,12 +43,13 @@ #include #include #include -#include #include #include -#include -#include +#include +#include +#include +#include #if !defined(UNUSED) # define UNUSED(a) ((void)(a)) @@ -109,7 +110,7 @@ class ToneAlarm : public cdev::CDev, public px4::ScheduledWorkItem unsigned int _silence_length{0}; ///< If nonzero, silence before next note. - int _tune_control_sub{-1}; + uORB::Subscription _tune_control_sub{ORB_ID(tune_control)}; tune_control_s _tune{}; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index bc504b84552f..702b239abdc9 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -71,12 +71,29 @@ add_compile_options(-Wno-cast-align) # TODO: fix and enable add_subdirectory(libuavcan EXCLUDE_FROM_ALL) add_dependencies(uavcan prebuild_targets) + +# generated DSDL +set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/dsdl/uavcan") +set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated") + +set(DSDLC_INPUT_FILES) +foreach(DSDLC_INPUT ${DSDLC_INPUTS}) + file(GLOB_RECURSE DSDLC_NEW_INPUT_FILES ${CMAKE_CURRENT_SOURCE_DIR} "${DSDLC_INPUT}/*.uavcan") + list(APPEND DSDLC_INPUT_FILES ${DSDLC_NEW_INPUT_FILES}) +endforeach(DSDLC_INPUT) +add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp + COMMAND ${PYTHON} ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT} + COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp + DEPENDS ${DSDLC_INPUT_FILES} + COMMENT "PX4 UAVCAN dsdl compiler" + ) +add_custom_target(px4_uavcan_dsdlc DEPENDS px4_uavcan_dsdlc_run.stamp) + px4_add_module( MODULE modules__uavcan MAIN uavcan - STACK_MAIN 3200 - STACK_MAX 1500 INCLUDES + ${DSDLC_OUTPUT} ${PX4_SOURCE_DIR}/mavlink/include/mavlink libuavcan/libuavcan/include libuavcan/libuavcan/include/dsdlc_generated @@ -97,15 +114,17 @@ px4_add_module( sensors/gnss.cpp sensors/mag.cpp sensors/baro.cpp + sensors/flow.cpp DEPENDS + px4_uavcan_dsdlc + mixer version git_uavcan # within libuavcan - libuavcan_dsdlc uavcan uavcan_${UAVCAN_PLATFORM}_driver ) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 2fbc670a63e8..de4a964b8fa5 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -43,6 +43,8 @@ #define MOTOR_BIT(x) (1<<(x)) +using namespace time_literals; + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -202,13 +204,10 @@ void UavcanEscController::arm_single_esc(int num, bool arm) void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) { if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) { - _esc_status.esc_count = uavcan::max(_esc_status.esc_count, msg.esc_index + 1); - _esc_status.timestamp = hrt_absolute_time(); - auto &ref = _esc_status.esc[msg.esc_index]; ref.esc_address = msg.getSrcNodeID().get(); - + ref.timestamp = hrt_absolute_time(); ref.esc_voltage = msg.voltage; ref.esc_current = msg.current; ref.esc_temperature = msg.temperature; @@ -220,8 +219,11 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure< void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) { + _esc_status.timestamp = hrt_absolute_time(); + _esc_status.esc_count = _rotor_count; _esc_status.counter += 1; _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_online_flags = UavcanEscController::check_escs_status(); if (_esc_status_pub != nullptr) { (void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status); @@ -230,3 +232,19 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &) _esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status); } } + +uint8_t UavcanEscController::check_escs_status() +{ + int esc_status_flags = 0; + hrt_abstime now = hrt_absolute_time(); + + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { + + if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) { + esc_status_flags |= (1 << index); + } + + } + + return esc_status_flags; +} diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index 071dbc190ca0..6001b5f5f64d 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/src/drivers/uavcan/actuators/esc.hpp @@ -50,6 +50,7 @@ #include #include #include +#include class UavcanEscController @@ -67,6 +68,13 @@ class UavcanEscController void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; } + /** + * Sets the number of rotors + */ + void set_rotor_count(uint8_t count) { _rotor_count = count; } + + static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable + private: /** * ESC status message reception will be reported via this callback. @@ -78,8 +86,11 @@ class UavcanEscController */ void orb_pub_timer_cb(const uavcan::TimerEvent &event); + /** + * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline. + */ + uint8_t check_escs_status(); - static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10; static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest @@ -95,6 +106,7 @@ class UavcanEscController esc_status_s _esc_status = {}; orb_advert_t _esc_status_pub = nullptr; orb_advert_t _actuator_outputs_pub = nullptr; + uint8_t _rotor_count = 0; /* * libuavcan related things diff --git a/src/drivers/uavcan/dsdl/com/hex/equipment/flow/20200.Measurement.uavcan b/src/drivers/uavcan/dsdl/com/hex/equipment/flow/20200.Measurement.uavcan new file mode 100644 index 000000000000..093a9a1c2b14 --- /dev/null +++ b/src/drivers/uavcan/dsdl/com/hex/equipment/flow/20200.Measurement.uavcan @@ -0,0 +1,4 @@ +float32 integration_interval +float32[2] rate_gyro_integral +float32[2] flow_integral +uint8 quality diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp new file mode 100644 index 000000000000..d2e6c25364fc --- /dev/null +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 "flow.hpp" + +#include + +const char *const UavcanFlowBridge::NAME = "flow"; + +UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_flow", "/dev/uavcan/flow", "/dev/flow", ORB_ID(optical_flow)), + _sub_flow(node) +{ +} + +int +UavcanFlowBridge::init() +{ + int res = device::CDev::init(); + + if (res < 0) { + return res; + } + + res = _sub_flow.start(FlowCbBinder(this, &UavcanFlowBridge::flow_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + return 0; +} + +void +UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + optical_flow_s flow{}; + + flow.timestamp = hrt_absolute_time(); + flow.pixel_flow_x_integral = msg.flow_integral[0]; + flow.pixel_flow_y_integral = msg.flow_integral[1]; + + flow.gyro_x_rate_integral = msg.rate_gyro_integral[0]; + flow.gyro_y_rate_integral = msg.rate_gyro_integral[1]; + + flow.quality = msg.quality; + flow.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s + flow.min_ground_distance = 0.1f; // Datasheet: 80mm + flow.max_ground_distance = 30.0f; // Datasheet: infinity + + publish(msg.getSrcNodeID().get(), &flow); +} diff --git a/src/drivers/uavcan/sensors/flow.hpp b/src/drivers/uavcan/sensors/flow.hpp new file mode 100644 index 000000000000..40a398a48eec --- /dev/null +++ b/src/drivers/uavcan/sensors/flow.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 "sensor_bridge.hpp" + +#include +#include + +#include + +#include + +class UavcanFlowBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanFlowBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void flow_sub_cb(const uavcan::ReceivedDataStructure &msg); + + typedef uavcan::MethodBinder < UavcanFlowBridge *, + void (UavcanFlowBridge::*) + (const uavcan::ReceivedDataStructure &) > + FlowCbBinder; + + uavcan::Subscriber _sub_flow; + +}; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index fec12209d62f..39cb57df2bc5 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -41,6 +41,7 @@ #include "gnss.hpp" #include "mag.hpp" #include "baro.hpp" +#include "flow.hpp" /* * IUavcanSensorBridge @@ -50,6 +51,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List= 0) { _poll_ids[i] = add_poll_fd(_control_subs[i]); + orb_set_interval(_control_subs[i], 1000 / UavcanEscController::MAX_RATE_HZ); } } } @@ -1123,6 +1124,11 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) } else { _mixers->groups_required(_groups_required); + PX4_INFO("Groups required %d", _groups_required); + + int rotor_count = _mixers->get_multirotor_count(); + _esc_controller.set_rotor_count(rotor_count); + PX4_INFO("Number of rotors %d", rotor_count); } } } diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index ed96586bc5ab..c47c739fc19c 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -564,20 +564,14 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) } // Acknowledge the received command - vehicle_command_ack_s ack = {}; + vehicle_command_ack_s ack{}; ack.timestamp = hrt_absolute_time(); ack.command = cmd.command; ack.result = cmd_ack_result; ack.target_system = cmd.source_system; ack.target_component = cmd.source_component; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack); - } - + _command_ack_pub.publish(ack); } // Shut down once armed diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index ec01ea50102c..7465e546c35e 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -36,7 +36,9 @@ #include #include "uavcan_driver.hpp" #include -#include +#include +#include +#include #include #include @@ -160,7 +162,7 @@ class __EXPORT UavcanServers // uORB topic handle for MAVLink parameter responses orb_advert_t _param_response_pub = nullptr; - orb_advert_t _command_ack_pub = nullptr; + uORB::PublicationQueued _command_ack_pub{ORB_ID(vehicle_command_ack)}; typedef uavcan::MethodBinder &)> GetSetCallback; diff --git a/src/examples/bottle_drop/CMakeLists.txt b/src/examples/bottle_drop/CMakeLists.txt index 7b0fc64cd672..9f02942cb993 100644 --- a/src/examples/bottle_drop/CMakeLists.txt +++ b/src/examples/bottle_drop/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE modules__bottle_drop MAIN bottle_drop - STACK_MAIN 1200 COMPILE_FLAGS SRCS bottle_drop.cpp diff --git a/src/examples/fixedwing_control/CMakeLists.txt b/src/examples/fixedwing_control/CMakeLists.txt index 3152597dfc9e..41b0a82856da 100644 --- a/src/examples/fixedwing_control/CMakeLists.txt +++ b/src/examples/fixedwing_control/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__fixedwing_control MAIN ex_fixedwing_control - STACK_MAIN 1200 STACK_MAX 1300 SRCS main.cpp diff --git a/src/examples/hello/hello_main.cpp b/src/examples/hello/hello_main.cpp index 75bff11cd167..227bc0ebc52d 100644 --- a/src/examples/hello/hello_main.cpp +++ b/src/examples/hello/hello_main.cpp @@ -40,8 +40,8 @@ #include "hello_example.h" -#include #include +#include #include int PX4_MAIN(int argc, char **argv) diff --git a/src/examples/hwtest/CMakeLists.txt b/src/examples/hwtest/CMakeLists.txt index eb0a77cb3d65..b9f3d3a9c8d3 100644 --- a/src/examples/hwtest/CMakeLists.txt +++ b/src/examples/hwtest/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__hwtest MAIN ex_hwtest - STACK_MAIN 2000 SRCS hwtest.c DEPENDS diff --git a/src/examples/matlab_csv_serial/CMakeLists.txt b/src/examples/matlab_csv_serial/CMakeLists.txt index 9a6202d1ea01..9cc4294a5bf5 100644 --- a/src/examples/matlab_csv_serial/CMakeLists.txt +++ b/src/examples/matlab_csv_serial/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__matlab_csv_serial MAIN matlab_csv_serial - STACK_MAIN 2000 SRCS matlab_csv_serial.c DEPENDS diff --git a/src/examples/position_estimator_inav/CMakeLists.txt b/src/examples/position_estimator_inav/CMakeLists.txt deleted file mode 100644 index 4311ccd05d13..000000000000 --- a/src/examples/position_estimator_inav/CMakeLists.txt +++ /dev/null @@ -1,47 +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 modules__position_estimator_inav - MAIN position_estimator_inav - STACK_MAIN 1200 - STACK_MAX 4000 - COMPILE_FLAGS - SRCS - position_estimator_inav_main.cpp - inertial_filter.cpp - DEPENDS - terrain_estimation - git_ecl - ecl_geo - ) diff --git a/src/examples/position_estimator_inav/inertial_filter.cpp b/src/examples/position_estimator_inav/inertial_filter.cpp deleted file mode 100644 index 4f045aeb6b5e..000000000000 --- a/src/examples/position_estimator_inav/inertial_filter.cpp +++ /dev/null @@ -1,34 +0,0 @@ -/* - * inertial_filter.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include "px4_defines.h" -#include "inertial_filter.h" -#include - -void inertial_filter_predict(float dt, float x[2], float acc) -{ - if (PX4_ISFINITE(dt)) { - if (!PX4_ISFINITE(acc)) { - acc = 0.0f; - } - - x[0] += x[1] * dt + acc * dt * dt / 2.0f; - x[1] += acc * dt; - } -} - -void inertial_filter_correct(float e, float dt, float x[2], int i, float w) -{ - if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) { - float ewdt = e * w * dt; - x[i] += ewdt; - - if (i == 0) { - x[1] += w * ewdt; - } - } -} diff --git a/src/examples/position_estimator_inav/inertial_filter.h b/src/examples/position_estimator_inav/inertial_filter.h deleted file mode 100644 index 5f8b40ef14d6..000000000000 --- a/src/examples/position_estimator_inav/inertial_filter.h +++ /dev/null @@ -1,15 +0,0 @@ -/* - * inertial_filter.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#pragma once - -#include -#include - -void inertial_filter_predict(float dt, float x[2], float acc); - -void inertial_filter_correct(float e, float dt, float x[2], int i, float w); diff --git a/src/examples/position_estimator_inav/params.c b/src/examples/position_estimator_inav/params.c deleted file mode 100644 index 2ae40b185ed0..000000000000 --- a/src/examples/position_estimator_inav/params.c +++ /dev/null @@ -1,346 +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 position_estimator_inav_params.c - * - * @author Anton Babushkin - * - * Parameters for position_estimator_inav - */ - -#include "position_estimator_inav_params.h" - -/** - * Z axis weight for barometer - * - * Weight (cutoff frequency) for barometer altitude measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); - -/** - * Z axis weight for GPS - * - * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); - -/** - * Z velocity weight for GPS - * - * Weight (cutoff frequency) for GPS altitude velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); - -/** - * Z axis weight for vision - * - * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); - -/** - * Z axis weight for lidar - * - * Weight (cutoff frequency) for lidar measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); - -/** - * XY axis weight for GPS position - * - * Weight (cutoff frequency) for GPS position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); - -/** - * XY axis weight for GPS velocity - * - * Weight (cutoff frequency) for GPS velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); - -/** - * XY axis weight for vision position - * - * Weight (cutoff frequency) for vision position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); - -/** - * XY axis weight for vision velocity - * - * Weight (cutoff frequency) for vision velocity measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); - -/** - * Weight for mocap system - * - * Weight (cutoff frequency) for mocap position measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ - -PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); - -/** - * XY axis weight for optical flow - * - * Weight (cutoff frequency) for optical flow (velocity) measurements. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 0.8f); - -/** - * XY axis weight for resetting velocity - * - * When velocity sources lost slowly decrease estimated horizontal velocity with this weight. - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f); - -/** - * XY axis weight factor for GPS when optical flow available - * - * When optical flow data available, multiply GPS weights (for position and velocity) by this factor. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); - -/** - * Accelerometer bias estimation weight - * - * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - * - * @min 0.0 - * @max 0.1 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); - -/** - * Optical flow scale factor - * - * Factor to scale optical flow - * - * @min 0.0 - * @max 10.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.35f); - -/** - * Minimal acceptable optical flow quality - * - * 0 - lowest quality, 1 - best quality. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); - -/** - * Sonar maximal error for new surface - * - * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable). - * - * @min 0.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.2f); - -/** - * Land detector time - * - * Vehicle assumed landed if no altitude changes happened during this time on low throttle. - * - * @min 0.0 - * @max 10.0 - * @unit s - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); - -/** - * Land detector altitude dispersion threshold - * - * Dispersion threshold for triggering land detector. - * - * @min 0.0 - * @max 10.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); - -/** - * Land detector throttle threshold - * - * Value should be lower than minimal hovering thrust. Half of it is good choice. - * - * @min 0.0 - * @max 1.0 - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); - -/** - * GPS delay - * - * GPS delay compensation - * - * @min 0.0 - * @max 1.0 - * @unit s - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); - -/** - * Flow module offset (center of rotation) in X direction - * - * Yaw X flow compensation - * - * @min -1.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); - -/** - * Flow module offset (center of rotation) in Y direction - * - * Yaw Y flow compensation - * - * @min -1.0 - * @max 1.0 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); - -/** - * Mo-cap - * - * Set to 0 if using fake GPS - * - * @value 0 Mo-cap enabled - * @value 1 Mo-cap disabled - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0); - -/** - * LIDAR for altitude estimation - * - * @boolean - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_EST, 0); - -/** - * LIDAR calibration offset - * - * LIDAR calibration offset. Value will be added to the measured distance - * - * @min -20 - * @max 20 - * @unit m - * @group Position Estimator INAV - */ -PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f); - -/** - * Disable vision input - * - * Set to the appropriate key (328754) to disable vision input. - * - * @reboot_required true - * @min 0 - * @max 328754 - * @category Developer - * @group Position Estimator INAV - */ -PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); diff --git a/src/examples/position_estimator_inav/position_estimator_inav_main.cpp b/src/examples/position_estimator_inav/position_estimator_inav_main.cpp deleted file mode 100644 index c7ff8c6e9a4e..000000000000 --- a/src/examples/position_estimator_inav/position_estimator_inav_main.cpp +++ /dev/null @@ -1,1599 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 position_estimator_inav_main.c - * Model-identification based position estimator for multirotors - * - * @author Anton Babushkin - * @author Nuno Marques - * @author Christoph Tobler - */ -#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 "position_estimator_inav_params.h" -#include "inertial_filter.h" - -#define MIN_VALID_W 0.00001f -#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz -#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s -#define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respond - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int position_estimator_inav_task; /**< Handle of deamon task / thread */ -static bool inav_verbose_mode = false; - -static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s -static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s -static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s -static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s -static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms -static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss -static const unsigned updates_counter_len = 1000000; -static const float max_flow = 1.0f; // max flow value that can be used, rad/s - -extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]); - -int position_estimator_inav_thread_main(int argc, char *argv[]); - -static void usage(const char *reason); - -static inline int min(int val1, int val2) -{ - return (val1 < val2) ? val1 : val2; -} - -static inline int max(int val1, int val2) -{ - return (val1 > val2) ? val1 : val2; -} - -/** - * Print the correct usage. - */ -static void usage(const char *reason) -{ - if (reason && *reason) { - PX4_INFO("%s", reason); - } - - PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n"); -} - -/** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int position_estimator_inav_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage("missing command"); - return -1; - } - - if (!strcmp(argv[1], "start")) { - if (thread_running) { - warnx("already running"); - /* this is not an error */ - return 0; - } - - inav_verbose_mode = false; - - if ((argc > 2) && (!strcmp(argv[2], "-v"))) { - inav_verbose_mode = true; - } - - thread_should_exit = false; - position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600, - position_estimator_inav_thread_main, - (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) nullptr); - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (thread_running) { - warnx("stop"); - thread_should_exit = true; - - } else { - warnx("not started"); - } - - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - return 0; - } - - usage("unrecognized command"); - return 1; -} - -#ifdef INAV_DEBUG -static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], - float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], - float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, - float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) -{ - FILE *f = fopen(PX4_STORAGEDIR"/inav.log", "a"); - - if (f) { - char *s = malloc(256); - unsigned n = snprintf(s, 256, - "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", - (unsigned long long)hrt_absolute_time(), msg, (double)dt, - (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], - (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], - (double)z_est_prev[1]); - fwrite(s, 1, n, f); - n = snprintf(s, 256, - "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n", - (double)acc[0], (double)acc[1], (double)acc[2], - (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], - (double)corr_gps[2][1], - (double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], - (double)w_mocap_p); - fwrite(s, 1, n, f); - n = snprintf(s, 256, - "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n", - (double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], - (double)corr_vision[1][1], (double)corr_vision[2][1], - (double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v); - fwrite(s, 1, n, f); - free(s); - } - - fsync(fileno(f)); - fclose(f); -} -#else -#define write_debug_log(...) -#endif - -/**************************************************************************** - * main - ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) -{ - orb_advert_t mavlink_log_pub = nullptr; - - float x_est[2] = { 0.0f, 0.0f }; // pos, vel - float y_est[2] = { 0.0f, 0.0f }; // pos, vel - float z_est[2] = { 0.0f, 0.0f }; // pos, vel - - float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer - float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer - float R_gps[3][3]; // rotation matrix for GPS correction moment - memset(est_buf, 0, sizeof(est_buf)); - memset(R_buf, 0, sizeof(R_buf)); - memset(R_gps, 0, sizeof(R_gps)); - int buf_ptr = 0; - - static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation - static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation - - float eph = max_eph_epv; - float epv = 1.0f; - - float eph_flow = 1.0f; - - float eph_vision = 0.2f; - float epv_vision = 0.2f; - - float eph_mocap = 0.05f; - float epv_mocap = 0.05f; - - float x_est_prev[2], y_est_prev[2], z_est_prev[2]; - memset(x_est_prev, 0, sizeof(x_est_prev)); - memset(y_est_prev, 0, sizeof(y_est_prev)); - memset(z_est_prev, 0, sizeof(z_est_prev)); - - int baro_init_cnt = 0; - int baro_init_num = 200; - float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted - - hrt_abstime accel_timestamp = 0; - hrt_abstime baro_timestamp = 0; - - bool ref_inited = false; - hrt_abstime ref_init_start = 0; - const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix - struct map_projection_reference_s ref; - memset(&ref, 0, sizeof(ref)); - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - uint16_t vision_updates = 0; - uint16_t mocap_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - hrt_abstime t_prev = 0; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D - float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float corr_baro = 0.0f; // D - float corr_gps[3][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - { 0.0f, 0.0f }, // D (pos, vel) - }; - float w_gps_xy = 1.0f; - float w_gps_z = 1.0f; - - float corr_vision[3][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - { 0.0f, 0.0f }, // D (pos, vel) - }; - - float corr_mocap[3][1] = { - { 0.0f }, // N (pos) - { 0.0f }, // E (pos) - { 0.0f }, // D (pos) - }; - const int mocap_heading = 2; - - float dist_ground = 0.0f; //variables for lidar altitude estimation - float corr_lidar = 0.0f; - float lidar_offset = 0.0f; - int lidar_offset_count = 0; - bool lidar_first = true; - bool use_lidar = false; - bool use_lidar_prev = false; - - float corr_flow[] = { 0.0f, 0.0f }; // N E - float w_flow = 0.0f; - - hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) - hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) - - int n_flow = 0; - float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; - float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; - float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; - float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; - float yaw_comp[] = { 0.0f, 0.0f }; - hrt_abstime flow_time = 0; - float flow_min_dist = 0.2f; - - bool gps_valid = false; // GPS is valid - bool lidar_valid = false; // lidar is valid - bool flow_valid = false; // flow is valid - bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) - bool vision_xy_valid = false; // vision XY is valid - bool vision_z_valid = false; // vision Z is valid - bool vision_vxy_valid = false; // vision VXY is valid - bool vision_vz_valid = false; // vision VZ is valid - bool mocap_xy_valid = false; // mocap XY is valid - bool mocap_z_valid = false; // mocap Z is valid - - /* set pose/velocity as invalid if standard deviation is bigger than max_std_dev */ - /* TODO: the user should be allowed to set these values by a parameter */ - static constexpr float ep_max_std_dev = 100.0f; // position estimation max std deviation - static constexpr float ev_max_std_dev = 100.0f; // velocity estimation max std deviation - - /* declare and safely initialize all structs */ - struct actuator_controls_s actuator; - memset(&actuator, 0, sizeof(actuator)); - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s pos; - memset(&pos, 0, sizeof(pos)); - struct optical_flow_s flow; - memset(&flow, 0, sizeof(flow)); - struct vehicle_odometry_s visual_odom; - memset(&visual_odom, 0, sizeof(visual_odom)); - struct vehicle_odometry_s mocap; - memset(&mocap, 0, sizeof(mocap)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct distance_sensor_s lidar; - memset(&lidar, 0, sizeof(lidar)); - struct vehicle_rates_setpoint_s rates_setpoint; - memset(&rates_setpoint, 0, sizeof(rates_setpoint)); - struct vehicle_air_data_s airdata; - memset(&airdata, 0, sizeof(vehicle_air_data_s)); - - /* subscribe */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); - int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int visual_odom_sub = orb_subscribe(ORB_ID(vehicle_visual_odometry)); - int mocap_position_sub = orb_subscribe(ORB_ID(vehicle_mocap_odometry)); - int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data)); - // because we can have several distance sensor instances with different orientations - int distance_sensor_subs[ORB_MULTI_MAX_INSTANCES]; - - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - distance_sensor_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i); - } - - /* advertise */ - orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - orb_advert_t vehicle_global_position_pub = nullptr; - - struct position_estimator_inav_params params; - memset(¶ms, 0, sizeof(params)); - struct position_estimator_inav_param_handles pos_inav_param_handles; - /* initialize parameter handles */ - inav_parameters_init(&pos_inav_param_handles); - - /* first parameters read at start up */ - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - ¶m_update); /* read from param topic to clear updated flag */ - /* first parameters update */ - inav_parameters_update(&pos_inav_param_handles, ¶ms); - - px4_pollfd_struct_t fds_init[1] = {}; - fds_init[0].fd = sensor_combined_sub; - fds_init[0].events = POLLIN; - - /* wait for initial baro value */ - bool wait_baro = true; - TerrainEstimator terrain_estimator; - - thread_running = true; - hrt_abstime baro_wait_for_sample_time = hrt_absolute_time(); - - while (wait_baro && !thread_should_exit) { - int ret = px4_poll(&fds_init[0], 1, 1000); - - if (ret < 0) { - /* poll error */ - mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); - - } else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { - wait_baro = false; - mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample"); - - } else if (ret > 0) { - if (fds_init[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - - bool baro_updated = false; - orb_check(vehicle_air_data_sub, &baro_updated); - - if (baro_updated) { - orb_copy(ORB_ID(vehicle_air_data), vehicle_air_data_sub, &airdata); - - if (wait_baro && airdata.timestamp != baro_timestamp) { - - baro_timestamp = airdata.timestamp; - baro_wait_for_sample_time = hrt_absolute_time(); - - /* mean calculation over several measurements */ - if (baro_init_cnt < baro_init_num) { - if (PX4_ISFINITE(airdata.baro_alt_meter)) { - baro_offset += airdata.baro_alt_meter; - baro_init_cnt++; - } - - } else { - wait_baro = false; - baro_offset /= (float) baro_init_cnt; - pos.z_valid = true; - pos.v_z_valid = true; - } - } - } - - - } - - } else { - PX4_WARN("INAV poll timeout"); - } - } - - /* main loop */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = vehicle_attitude_sub; - fds[0].events = POLLIN; - - while (!thread_should_exit) { - int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate - hrt_abstime t = hrt_absolute_time(); - - if (ret < 0) { - /* poll error */ - mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); - continue; - - } else if (ret > 0) { - /* act on attitude updates */ - - /* vehicle attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; - - bool updated; - - /* parameter update */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - inav_parameters_update(&pos_inav_param_handles, ¶ms); - } - - /* actuator */ - orb_check(actuator_sub, &updated); - - if (updated) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); - } - - /* armed */ - orb_check(armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } - - /* sensor combined */ - orb_check(sensor_combined_sub, &updated); - - matrix::Dcmf R = matrix::Quatf(att.q); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - - if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) { - /* correct accel bias */ - sensor.accelerometer_m_s2[0] -= acc_bias[0]; - sensor.accelerometer_m_s2[1] -= acc_bias[1]; - sensor.accelerometer_m_s2[2] -= acc_bias[2]; - - /* transform acceleration vector from body frame to NED frame */ - for (int i = 0; i < 3; i++) { - acc[i] = 0.0f; - - for (int j = 0; j < 3; j++) { - acc[i] += R(i, j) * sensor.accelerometer_m_s2[j]; - } - } - - acc[2] += CONSTANTS_ONE_G; - - accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative; - accel_updates++; - } - - if (airdata.timestamp != baro_timestamp) { - corr_baro = baro_offset - airdata.baro_alt_meter - z_est[0]; - baro_timestamp = airdata.timestamp; - baro_updates++; - } - } - - - /* lidar alt estimation - * update lidar separately, needed by terrain estimator */ - for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - - orb_check(distance_sensor_subs[i], &updated); - - if (updated) { - - orb_copy(ORB_ID(distance_sensor), distance_sensor_subs[i], &lidar); - - if (lidar.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) { - updated = false; - - } else { - lidar.current_distance += params.lidar_calibration_offset; - break; // only the first valid distance sensor instance is used - } - } - } - - if (updated) { //check if altitude estimation for lidar is enabled and new sensor data - - if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance - && lidar.current_distance < lidar.max_distance - && (R(2, 2) > 0.7f)) { - - if (!use_lidar_prev && use_lidar) { - lidar_first = true; - } - - use_lidar_prev = use_lidar; - - lidar_time = t; - dist_ground = lidar.current_distance * R(2, 2); //vertical distance - - if (lidar_first) { - lidar_first = false; - lidar_offset = dist_ground + z_est[0]; - mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset"); - warnx("[inav] LIDAR: new ground offset"); - } - - corr_lidar = lidar_offset - dist_ground - z_est[0]; - - if (fabsf(corr_lidar) > params.lidar_err) { //check for spike - corr_lidar = 0; - lidar_valid = false; - lidar_offset_count++; - - if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit - lidar_first = true; - lidar_offset_count = 0; - } - - } else { - corr_lidar = lidar_offset - dist_ground - z_est[0]; - lidar_valid = true; - lidar_offset_count = 0; - lidar_valid_time = t; - } - - } else { - lidar_valid = false; - } - } - - /* optical flow */ - orb_check(optical_flow_sub, &updated); - - if (updated && lidar_valid) { - orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - - flow_time = t; - float flow_q = flow.quality / 255.0f; - float dist_bottom = lidar.current_distance; - - if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && R(2, 2) > 0.7f) { - /* distance to surface */ - //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar - float flow_dist = dist_bottom; //use this if using lidar - - /* check if flow if too large for accurate measurements */ - /* calculate estimated velocity in body frame */ - float body_v_est[2] = { 0.0f, 0.0f }; - - for (int i = 0; i < 2; i++) { - body_v_est[i] = R(0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1]; - } - - /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ - flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && - fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; - - /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ - flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f; - flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f; - flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f; - - //moving average - if (n_flow >= 100) { - gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; - gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; - gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; - n_flow = 0; - flow_gyrospeed_filtered[0] = 0.0f; - flow_gyrospeed_filtered[1] = 0.0f; - flow_gyrospeed_filtered[2] = 0.0f; - att_gyrospeed_filtered[0] = 0.0f; - att_gyrospeed_filtered[1] = 0.0f; - att_gyrospeed_filtered[2] = 0.0f; - - } else { - flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); - flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); - flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); - att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); - att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); - att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); - n_flow++; - } - - - /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ - yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); - yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); - - /* - * Convert raw flow from the optical_flow uORB topic (rad) to angular flow (rad/s) - * Note that the optical_flow uORB topic defines positive delta angles as produced by RH rotations - * around the correspdonding body axes. - */ - - float flow_ang[2]; - - /* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */ - orb_check(vehicle_rate_sp_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); - } - - float rate_threshold = 0.15f; - - /* calculate the angular flow rate produced by a negative velocity along the X body axis */ - if (fabsf(rates_setpoint.pitch) < rate_threshold) { - //warnx("[inav] test ohne comp"); - flow_ang[0] = (-flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * - params.flow_k;//for now the flow has to be scaled (to small) - - } else { - //warnx("[inav] test mit comp"); - //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[0] = (-(flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f - + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) - } - - /* calculate the angular flow rate produced by a negative velocity along the Y body axis */ - if (fabsf(rates_setpoint.roll) < rate_threshold) { - flow_ang[1] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * - params.flow_k;//for now the flow has to be scaled (to small) - - } else { - //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[1] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f - + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) - } - - /* flow measurements vector */ - float flow_m[3]; - - if (fabsf(rates_setpoint.yaw) < rate_threshold) { - flow_m[0] = -flow_ang[0] * flow_dist; - flow_m[1] = -flow_ang[1] * flow_dist; - - } else { - flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; - flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; - } - - flow_m[2] = z_est[1]; - - /* velocity in NED */ - float flow_v[2] = { 0.0f, 0.0f }; - - /* project measurements vector to NED basis, skip Z component */ - for (int i = 0; i < 2; i++) { - for (int j = 0; j < 3; j++) { - flow_v[i] += R(i, j) * flow_m[j]; - } - } - - /* velocity correction */ - corr_flow[0] = flow_v[0] - x_est[1]; - corr_flow[1] = flow_v[1] - y_est[1]; - /* adjust correction weight */ - float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); - w_flow = R(2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); - - - /* if flow is not accurate, reduce weight for it */ - // TODO make this more fuzzy - if (!flow_accurate) { - w_flow *= 0.05f; - } - - /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1f / w_flow; - - flow_valid = true; - - } else { - w_flow = 0.0f; - flow_valid = false; - } - - flow_updates++; - } - - /* check no vision circuit breaker is set */ - if (params.no_vision != CBRK_NO_VISION_KEY) { - /* vehicle visual odometry */ - orb_check(visual_odom_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_visual_odometry), visual_odom_sub, &visual_odom); - - static float last_vision_x = 0.0f; - static float last_vision_y = 0.0f; - static float last_vision_z = 0.0f; - - vision_xy_valid = PX4_ISFINITE(visual_odom.x) - && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE], - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); - vision_z_valid = PX4_ISFINITE(visual_odom.z) - && (PX4_ISFINITE(visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_X_VARIANCE]) ? - visual_odom.pose_covariance[visual_odom.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); - vision_vxy_valid = PX4_ISFINITE(visual_odom.vx) - && PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? sqrtf( - fmaxf(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE], - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VY_VARIANCE])) <= ev_max_std_dev : true; - vision_vz_valid = PX4_ISFINITE(visual_odom.vz) - && (PX4_ISFINITE(visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VX_VARIANCE]) ? - visual_odom.velocity_covariance[visual_odom.COVARIANCE_MATRIX_VZ_VARIANCE] <= ep_max_std_dev : true); - - /* reset position estimate on first vision update */ - if (vision_xy_valid) { - x_est[0] = visual_odom.x; - y_est[0] = visual_odom.y; - - last_vision_x = visual_odom.x; - last_vision_y = visual_odom.y; - - } else { - warnx("VISION XY estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION XY estimate not valid"); - - } - - if (vision_vxy_valid) { - x_est[1] = visual_odom.vx; - y_est[1] = visual_odom.vy; - - } else { - warnx("VISION VXY estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION VXY estimate not valid"); - - } - - /* only reset the z estimate if the z weight parameter is not zero */ - if (params.w_z_vision_p > MIN_VALID_W) { - if (vision_z_valid) { - z_est[0] = visual_odom.z; - - last_vision_z = visual_odom.z; - - } else { - warnx("VISION Z estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION Z estimate not valid"); - - } - - if (vision_vz_valid) { - z_est[1] = visual_odom.vz; - - } else { - warnx("VISION VZ estimate not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION VZ estimate not valid"); - - } - } - - /* calculate correction for position */ - if (vision_xy_valid) { - corr_vision[0][0] = visual_odom.x - x_est[0]; - corr_vision[1][0] = visual_odom.y - y_est[0]; - } - - if (vision_z_valid) { - corr_vision[2][0] = visual_odom.z - z_est[0]; - } - - static hrt_abstime last_vision_time = 0; - - float vision_dt = (visual_odom.timestamp - last_vision_time) / 1e6f; - last_vision_time = visual_odom.timestamp; - - if (vision_vxy_valid) { - /* calculate correction for XY velocity from external estimation */ - corr_vision[0][1] = visual_odom.vx - x_est[1]; - corr_vision[1][1] = visual_odom.vy - y_est[1]; - - } else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_xy_valid) { - visual_odom.vx = (visual_odom.x - last_vision_x) / vision_dt; - visual_odom.vy = (visual_odom.y - last_vision_y) / vision_dt; - - last_vision_x = visual_odom.x; - last_vision_y = visual_odom.y; - - /* calculate correction for XY velocity */ - corr_vision[0][1] = visual_odom.vx - x_est[1]; - corr_vision[1][1] = visual_odom.vy - y_est[1]; - - } else { - /* assume zero motion in XY plane */ - corr_vision[0][1] = 0.0f - x_est[1]; - corr_vision[1][1] = 0.0f - y_est[1]; - - } - - if (vision_vz_valid) { - /* calculate correction for Z velocity from external estimation */ - corr_vision[2][1] = visual_odom.vz - z_est[1]; - - } else if (vision_dt > 0.000001f && vision_dt < 0.2f && vision_z_valid) { - visual_odom.vz = (visual_odom.z - last_vision_z) / vision_dt; - - last_vision_z = visual_odom.z; - - /* calculate correction for Z velocity */ - corr_vision[2][1] = visual_odom.vz - z_est[1]; - - } else { - /* assume zero motion in Z plane */ - corr_vision[2][1] = 0.0f - z_est[1]; - } - - vision_updates++; - } - } - - /* vehicle mocap position */ - orb_check(mocap_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_mocap_odometry), mocap_position_sub, &mocap); - - mocap_xy_valid = PX4_ISFINITE(mocap.x) - && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? sqrtf(fmaxf( - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE], - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Y_VARIANCE])) <= ep_max_std_dev : true); - mocap_z_valid = PX4_ISFINITE(mocap.z) - && (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_X_VARIANCE]) ? - mocap.pose_covariance[mocap.COVARIANCE_MATRIX_Z_VARIANCE] <= ep_max_std_dev : true); - - if (!params.disable_mocap) { - /* reset position estimate on first mocap update */ - if (mocap_xy_valid) { - x_est[0] = mocap.x; - y_est[0] = mocap.y; - } - - if (mocap_z_valid) { - z_est[0] = mocap.z; - } - - if (!mocap_xy_valid || !mocap_z_valid) { - warnx("MOCAP data not valid"); - mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data not valid");; - - } - - /* calculate correction for position */ - if (mocap_xy_valid) { - corr_mocap[0][0] = mocap.x - x_est[0]; - corr_mocap[1][0] = mocap.y - y_est[0]; - } - - if (mocap_z_valid) { - corr_mocap[2][0] = mocap.z - z_est[0]; - } - - mocap_updates++; - } - } - - /* vehicle GPS position */ - orb_check(vehicle_gps_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - bool reset_est = false; - - /* hysteresis for GPS quality */ - if (gps_valid) { - if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { - gps_valid = false; - mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost"); - warnx("[inav] GPS signal lost"); - } - - } else { - if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { - gps_valid = true; - reset_est = true; - mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found"); - warnx("[inav] GPS signal found"); - } - } - - if (gps_valid) { - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - float alt = gps.alt * 1e-3; - - /* initialize reference position if needed */ - if (!ref_inited) { - if (ref_init_start == 0) { - ref_init_start = t; - - } else if (t > ref_init_start + ref_init_delay) { - ref_inited = true; - - /* set position estimate to (0, 0, 0), use GPS velocity for XY */ - x_est[0] = 0.0f; - x_est[1] = gps.vel_n_m_s; - y_est[0] = 0.0f; - y_est[1] = gps.vel_e_m_s; - - pos.ref_lat = lat; - pos.ref_lon = lon; - pos.ref_alt = alt + z_est[0]; - pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(&ref, lat, lon); - // XXX replace this print - warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); - } - } - - if (ref_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); - - /* reset position estimate when GPS becomes good */ - if (reset_est) { - x_est[0] = gps_proj[0]; - x_est[1] = gps.vel_n_m_s; - y_est[0] = gps_proj[1]; - y_est[1] = gps.vel_e_m_s; - } - - /* calculate index of estimated values in buffer */ - int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); - - if (est_i < 0) { - est_i += EST_BUF_SIZE; - } - - /* calculate correction for position */ - corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; - corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; - corr_gps[2][0] = pos.ref_alt - alt - est_buf[est_i][2][0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; - corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; - corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; - - } else { - corr_gps[0][1] = 0.0f; - corr_gps[1][1] = 0.0f; - corr_gps[2][1] = 0.0f; - } - - /* save rotation matrix at this moment */ - memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); - } - - } else { - /* no GPS lock */ - memset(corr_gps, 0, sizeof(corr_gps)); - ref_init_start = 0; - } - - gps_updates++; - } - } - - matrix::Dcm R = matrix::Quatf(att.q); - - /* check for timeout on FLOW topic */ - if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) { - flow_valid = false; - warnx("FLOW timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout"); - } - - /* check for timeout on GPS topic */ - if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) { - gps_valid = false; - warnx("GPS timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); - } - - /* check for timeout on vision topic */ - if ((vision_xy_valid || vision_z_valid) && (t > (visual_odom.timestamp + vision_topic_timeout))) { - vision_xy_valid = false; - vision_z_valid = false; - warnx("VISION timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); - } - - /* check for timeout on mocap topic */ - if ((mocap_xy_valid || mocap_z_valid) && (t > (mocap.timestamp + mocap_topic_timeout))) { - mocap_xy_valid = false; - mocap_z_valid = false; - warnx("MOCAP timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); - } - - /* check for lidar measurement timeout */ - if (lidar_valid && (t > (lidar_time + lidar_timeout))) { - lidar_valid = false; - warnx("LIDAR timeout"); - mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout"); - } - - float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms - t_prev = t; - - /* increase EPH/EPV on each step */ - if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 - eph = 0.001; - } - - if (eph < max_eph_epv) { - eph *= 1.0f + dt; - } - - if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 - epv = 0.001; - } - - if (epv < max_eph_epv) { - epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) - } - - /* use GPS if it's valid and reference position initialized */ - bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; - bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; - /* use VISION if it's valid and has a valid weight parameter */ - bool use_vision_xy = vision_xy_valid && params.w_xy_vision_p > MIN_VALID_W; - bool use_vision_z = vision_z_valid && params.w_z_vision_p > MIN_VALID_W; - /* use MOCAP if it's valid and has a valid weight parameter */ - bool use_mocap = mocap_xy_valid && mocap_z_valid && params.w_mocap_p > MIN_VALID_W - && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap - - if (params.disable_mocap) { //disable mocap if fake gps is used - use_mocap = false; - } - - /* use flow if it's valid and (accurate or no GPS available) */ - bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - - /* use LIDAR if it's valid and lidar altitude estimation is enabled */ - use_lidar = lidar_valid && params.enable_lidar_alt_est; - - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - - bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); - - float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; - float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; - float w_z_gps_p = params.w_z_gps_p * w_gps_z; - float w_z_gps_v = params.w_z_gps_v * w_gps_z; - - float w_xy_vision_p = params.w_xy_vision_p; - float w_xy_vision_v = params.w_xy_vision_v; - float w_z_vision_p = params.w_z_vision_p; - - float w_mocap_p = params.w_mocap_p; - - /* reduce GPS weight if optical flow is good */ - if (use_flow && flow_accurate) { - w_xy_gps_p *= params.w_gps_flow; - w_xy_gps_v *= params.w_gps_flow; - } - - /* baro offset correction */ - if (use_gps_z) { - float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; - baro_offset += offs_corr; - corr_baro += offs_corr; - } - - /* accelerometer bias correction for GPS (use buffered rotation matrix) */ - float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; - - if (use_gps_xy) { - accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; - accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; - accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; - accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; - } - - if (use_gps_z) { - accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; - accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R_gps[j][i] * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* accelerometer bias correction for VISION (use buffered rotation matrix) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_vision_xy) { - accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; - accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; - accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; - accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; - } - - if (use_vision_z) { - accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; - } - - /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_mocap) { - accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; - accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; - accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R(j, i) * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* accelerometer bias correction for flow and baro (assume that there is no delay) */ - accel_bias_corr[0] = 0.0f; - accel_bias_corr[1] = 0.0f; - accel_bias_corr[2] = 0.0f; - - if (use_flow) { - accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; - accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; - } - - if (use_lidar) { - accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar; - - } else { - accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; - } - - /* transform error vector from NED frame to body frame */ - for (int i = 0; i < 3; i++) { - float c = 0.0f; - - for (int j = 0; j < 3; j++) { - c += R(j, i) * accel_bias_corr[j]; - } - - if (PX4_ISFINITE(c)) { - acc_bias[i] += c * params.w_acc_bias * dt; - } - } - - /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est, acc[2]); - - if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(z_est, z_est_prev, sizeof(z_est)); - } - - /* inertial filter correction for altitude */ - if (use_lidar) { - inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar); - - } else { - inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); - } - - if (use_gps_z) { - epv = fminf(epv, gps.epv); - - inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); - inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); - } - - if (use_vision_z) { - epv = fminf(epv, epv_vision); - inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); - } - - if (use_mocap) { - epv = fminf(epv, epv_mocap); - inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); - } - - if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(z_est, z_est_prev, sizeof(z_est)); - memset(corr_gps, 0, sizeof(corr_gps)); - memset(corr_vision, 0, sizeof(corr_vision)); - memset(corr_mocap, 0, sizeof(corr_mocap)); - corr_baro = 0; - - } else { - memcpy(z_est_prev, z_est, sizeof(z_est)); - } - - if (can_estimate_xy) { - /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est, acc[0]); - inertial_filter_predict(dt, y_est, acc[1]); - - if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(x_est, x_est_prev, sizeof(x_est)); - memcpy(y_est, y_est_prev, sizeof(y_est)); - } - - /* inertial filter correction for position */ - if (use_flow) { - eph = fminf(eph, eph_flow); - - inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); - inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); - } - - if (use_gps_xy) { - eph = fminf(eph, gps.eph); - - inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); - inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); - - if (gps.vel_ned_valid && t < gps.timestamp + gps_topic_timeout) { - inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); - inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); - } - } - - if (use_vision_xy) { - eph = fminf(eph, eph_vision); - - inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); - inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); - - if (w_xy_vision_v > MIN_VALID_W) { - inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); - inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); - } - } - - if (use_mocap) { - eph = fminf(eph, eph_mocap); - - inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); - inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); - } - - if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, - acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, - corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); - memcpy(x_est, x_est_prev, sizeof(x_est)); - memcpy(y_est, y_est_prev, sizeof(y_est)); - memset(corr_gps, 0, sizeof(corr_gps)); - memset(corr_vision, 0, sizeof(corr_vision)); - memset(corr_mocap, 0, sizeof(corr_mocap)); - memset(corr_flow, 0, sizeof(corr_flow)); - - } else { - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); - } - - } else { - /* gradually reset xy velocity estimates */ - inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); - inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); - } - - /* run terrain estimator */ - terrain_estimator.predict(dt, &att, &sensor, &lidar); - terrain_estimator.measurement_update(hrt_absolute_time(), &gps, &lidar, &att); - - if (inav_verbose_mode) { - /* print updates rate */ - if (t > updates_counter_start + updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - warnx( - "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", - (double)(accel_updates / updates_dt), - (double)(baro_updates / updates_dt), - (double)(gps_updates / updates_dt), - (double)(attitude_updates / updates_dt), - (double)(flow_updates / updates_dt), - (double)(vision_updates / updates_dt), - (double)(mocap_updates / updates_dt)); - updates_counter_start = t; - accel_updates = 0; - baro_updates = 0; - gps_updates = 0; - attitude_updates = 0; - flow_updates = 0; - vision_updates = 0; - mocap_updates = 0; - } - } - - if (t > pub_last + PUB_INTERVAL) { - pub_last = t; - - /* push current estimate to buffer */ - est_buf[buf_ptr][0][0] = x_est[0]; - est_buf[buf_ptr][0][1] = x_est[1]; - est_buf[buf_ptr][1][0] = y_est[0]; - est_buf[buf_ptr][1][1] = y_est[1]; - est_buf[buf_ptr][2][0] = z_est[0]; - est_buf[buf_ptr][2][1] = z_est[1]; - - /* push current rotation matrix to buffer */ - memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data)); - - buf_ptr++; - - if (buf_ptr >= EST_BUF_SIZE) { - buf_ptr = 0; - } - - - /* publish local position */ - pos.xy_valid = can_estimate_xy; - pos.v_xy_valid = can_estimate_xy; - pos.xy_global = pos.xy_valid && use_gps_xy; - pos.z_global = pos.z_valid && use_gps_z; - pos.x = x_est[0]; - pos.vx = x_est[1]; - pos.y = y_est[0]; - pos.vy = y_est[1]; - pos.z = z_est[0]; - pos.vz = z_est[1]; - pos.ax = NAN; - pos.ay = NAN; - pos.az = NAN; - pos.yaw = matrix::Eulerf(matrix::Quatf(att.q)).psi(); - pos.dist_bottom_valid = dist_bottom_valid; - pos.eph = eph; - pos.epv = epv; - pos.evh = 0.0f; - pos.evv = 0.0f; - pos.vxy_max = INFINITY; - pos.vz_max = INFINITY; - pos.hagl_min = INFINITY; - pos.hagl_max = INFINITY; - - // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity - pos.z_deriv = z_est[1]; - - if (pos.dist_bottom_valid) { - pos.dist_bottom = dist_ground; - pos.dist_bottom_rate = - z_est[1]; - } - - pos.timestamp = t; - - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); - - if (pos.xy_global && pos.z_global) { - /* publish global position */ - global_pos.timestamp = t; - - double est_lat, est_lon; - map_projection_reproject(&ref, pos.x, pos.y, &est_lat, &est_lon); - - global_pos.lat = est_lat; - global_pos.lon = est_lon; - global_pos.alt = pos.ref_alt - pos.z; - - global_pos.vel_n = pos.vx; - global_pos.vel_e = pos.vy; - global_pos.vel_d = pos.vz; - - global_pos.yaw = matrix::Eulerf(matrix::Quatf(R)).psi(); - - global_pos.eph = eph; - global_pos.epv = epv; - - if (terrain_estimator.is_valid()) { - global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground(); - global_pos.terrain_alt_valid = true; - - } else { - global_pos.terrain_alt_valid = false; - } - - if (vehicle_global_position_pub == nullptr) { - vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } - } - } - } - - warnx("stopped"); - mavlink_log_info(&mavlink_log_pub, "[inav] stopped"); - thread_running = false; - return 0; -} - - -int inav_parameters_init(struct position_estimator_inav_param_handles *h) -{ - h->w_z_baro = param_find("INAV_W_Z_BARO"); - h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); - h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); - h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); - h->w_z_lidar = param_find("INAV_W_Z_LIDAR"); - h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); - h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); - h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); - h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); - h->w_mocap_p = param_find("INAV_W_MOC_P"); - h->w_xy_flow = param_find("INAV_W_XY_FLOW"); - h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); - h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); - h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); - h->flow_k = param_find("INAV_FLOW_K"); - h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); - h->lidar_err = param_find("INAV_LIDAR_ERR"); - h->land_t = param_find("INAV_LAND_T"); - h->land_disp = param_find("INAV_LAND_DISP"); - h->land_thr = param_find("INAV_LAND_THR"); - h->no_vision = param_find("CBRK_NO_VISION"); - h->delay_gps = param_find("INAV_DELAY_GPS"); - h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); - h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); - h->disable_mocap = param_find("INAV_DISAB_MOCAP"); - h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST"); - h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF"); - h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M"); - - return 0; -} - -int inav_parameters_update(const struct position_estimator_inav_param_handles *h, - struct position_estimator_inav_params *p) -{ - param_get(h->w_z_baro, &(p->w_z_baro)); - param_get(h->w_z_gps_p, &(p->w_z_gps_p)); - param_get(h->w_z_gps_v, &(p->w_z_gps_v)); - param_get(h->w_z_vision_p, &(p->w_z_vision_p)); - param_get(h->w_z_lidar, &(p->w_z_lidar)); - param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); - param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); - param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); - param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); - param_get(h->w_mocap_p, &(p->w_mocap_p)); - param_get(h->w_xy_flow, &(p->w_xy_flow)); - param_get(h->w_xy_res_v, &(p->w_xy_res_v)); - param_get(h->w_gps_flow, &(p->w_gps_flow)); - param_get(h->w_acc_bias, &(p->w_acc_bias)); - param_get(h->flow_k, &(p->flow_k)); - param_get(h->flow_q_min, &(p->flow_q_min)); - param_get(h->lidar_err, &(p->lidar_err)); - param_get(h->land_t, &(p->land_t)); - param_get(h->land_disp, &(p->land_disp)); - param_get(h->land_thr, &(p->land_thr)); - param_get(h->no_vision, &(p->no_vision)); - param_get(h->delay_gps, &(p->delay_gps)); - param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); - param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); - param_get(h->disable_mocap, &(p->disable_mocap)); - param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est)); - param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset)); - param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m)); - - return 0; -} diff --git a/src/examples/position_estimator_inav/position_estimator_inav_params.h b/src/examples/position_estimator_inav/position_estimator_inav_params.h deleted file mode 100644 index d8e6de7a16de..000000000000 --- a/src/examples/position_estimator_inav/position_estimator_inav_params.h +++ /dev/null @@ -1,121 +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 position_estimator_inav_params.c - * - * @author Anton Babushkin - * - * Parameters definition for position_estimator_inav - */ - -#pragma once - -#include - -struct position_estimator_inav_params { - float w_z_baro; - float w_z_gps_p; - float w_z_gps_v; - float w_z_vision_p; - float w_z_lidar; - float w_xy_gps_p; - float w_xy_gps_v; - float w_xy_vision_p; - float w_xy_vision_v; - float w_mocap_p; - float w_xy_flow; - float w_xy_res_v; - float w_gps_flow; - float w_acc_bias; - float flow_k; - float flow_q_min; - float lidar_err; - float land_t; - float land_disp; - float land_thr; - int32_t no_vision; - float delay_gps; - float flow_module_offset_x; - float flow_module_offset_y; - int32_t disable_mocap; - int32_t enable_lidar_alt_est; - float lidar_calibration_offset; - int32_t att_ext_hdg_m; -}; - -struct position_estimator_inav_param_handles { - param_t w_z_baro; - param_t w_z_gps_p; - param_t w_z_gps_v; - param_t w_z_vision_p; - param_t w_z_lidar; - param_t w_xy_gps_p; - param_t w_xy_gps_v; - param_t w_xy_vision_p; - param_t w_xy_vision_v; - param_t w_mocap_p; - param_t w_xy_flow; - param_t w_xy_res_v; - param_t w_gps_flow; - param_t w_acc_bias; - param_t flow_k; - param_t flow_q_min; - param_t lidar_err; - param_t land_t; - param_t land_disp; - param_t land_thr; - param_t no_vision; - param_t delay_gps; - param_t flow_module_offset_x; - param_t flow_module_offset_y; - param_t disable_mocap; - param_t enable_lidar_alt_est; - param_t lidar_calibration_offset; - param_t att_ext_hdg_m; -}; - -#define CBRK_NO_VISION_KEY 328754 - -/** - * Initialize all parameter handles and values - * - */ -int inav_parameters_init(struct position_estimator_inav_param_handles *h); - -/** - * Update all parameters - * - */ -int inav_parameters_update(const struct position_estimator_inav_param_handles *h, - struct position_estimator_inav_params *p); diff --git a/src/examples/px4_mavlink_debug/CMakeLists.txt b/src/examples/px4_mavlink_debug/CMakeLists.txt index a9a0ca724f87..35c91a27e1fc 100644 --- a/src/examples/px4_mavlink_debug/CMakeLists.txt +++ b/src/examples/px4_mavlink_debug/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__px4_mavlink_debug MAIN px4_mavlink_debug - STACK_MAIN 2000 SRCS px4_mavlink_debug.cpp DEPENDS diff --git a/src/examples/px4_simple_app/CMakeLists.txt b/src/examples/px4_simple_app/CMakeLists.txt index 5685369f276b..6a91bc47e1e4 100644 --- a/src/examples/px4_simple_app/CMakeLists.txt +++ b/src/examples/px4_simple_app/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__px4_simple_app MAIN px4_simple_app - STACK_MAIN 2000 SRCS px4_simple_app.c DEPENDS diff --git a/src/examples/rover_steering_control/CMakeLists.txt b/src/examples/rover_steering_control/CMakeLists.txt index 8afaeae2c56a..74213b77291a 100644 --- a/src/examples/rover_steering_control/CMakeLists.txt +++ b/src/examples/rover_steering_control/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__rover_steering_control MAIN rover_steering_control - STACK_MAIN 1200 STACK_MAX 1300 SRCS main.cpp diff --git a/src/examples/segway/BlockSegwayController.cpp b/src/examples/segway/BlockSegwayController.cpp index e3fefe171c53..bacb7f7c1a4f 100644 --- a/src/examples/segway/BlockSegwayController.cpp +++ b/src/examples/segway/BlockSegwayController.cpp @@ -42,7 +42,7 @@ void BlockSegwayController::update() Eulerf euler = Eulerf(Quatf(_att.get().q)); // compute speed command - float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed); + float spdCmd = -th2v.update(euler.theta()) - q2v.update(_angular_velocity.get().xyz[1]); // handle autopilot modes if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || diff --git a/src/examples/segway/CMakeLists.txt b/src/examples/segway/CMakeLists.txt index caf2b4401b0d..11dd8e917f47 100644 --- a/src/examples/segway/CMakeLists.txt +++ b/src/examples/segway/CMakeLists.txt @@ -33,8 +33,6 @@ px4_add_module( MODULE examples__segway MAIN segway - STACK_MAIN 1400 - STACK_MAX 1400 SRCS blocks.cpp segway_main.cpp diff --git a/src/examples/segway/blocks.cpp b/src/examples/segway/blocks.cpp index 2112ddb0db2b..a6348805e5c8 100644 --- a/src/examples/segway/blocks.cpp +++ b/src/examples/segway/blocks.cpp @@ -89,6 +89,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), + _angular_velocity(ORB_ID(vehicle_angular_velocity), 20, 0, &getSubscriptions()), _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), diff --git a/src/examples/segway/blocks.hpp b/src/examples/segway/blocks.hpp index 4557a76479f5..db521b975443 100644 --- a/src/examples/segway/blocks.hpp +++ b/src/examples/segway/blocks.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -96,6 +97,7 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock uORB::SubscriptionPollable _param_update; uORB::SubscriptionPollable _missionCmd; uORB::SubscriptionPollable _att; + uORB::SubscriptionPollable _angular_velocity; uORB::SubscriptionPollable _attCmd; uORB::SubscriptionPollable _pos; uORB::SubscriptionPollable _ratesCmd; diff --git a/src/examples/uuv_example_app/CMakeLists.txt b/src/examples/uuv_example_app/CMakeLists.txt index bcfa534b4b40..647b52168c50 100644 --- a/src/examples/uuv_example_app/CMakeLists.txt +++ b/src/examples/uuv_example_app/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE examples__uuv_example_app MAIN uuv_example_app - STACK_MAIN 2000 SRCS uuv_example_app.cpp DEPENDS diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 5cf7447beb3b..119efa207150 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -68,13 +68,14 @@ class List bool remove(T removeNode) { + if (removeNode == nullptr) { + return false; + } + // base case if (removeNode == _head) { - if (_head->getSibling() != nullptr) { + if (_head != nullptr) { _head = _head->getSibling(); - - } else { - _head = nullptr; } return true; diff --git a/src/lib/AirspeedValidator/AirspeedValidator.cpp b/src/lib/AirspeedValidator/AirspeedValidator.cpp new file mode 100644 index 000000000000..027bc9b96125 --- /dev/null +++ b/src/lib/AirspeedValidator/AirspeedValidator.cpp @@ -0,0 +1,254 @@ +/**************************************************************************** + * + * 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 AirspeedValidator.cpp + * Estimates airspeed scale error (from indicated to equivalent airspeed), performes + * checks on airspeed measurement input and reports airspeed valid or invalid. + */ + +#include "AirspeedValidator.hpp" + + +void +AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_data &input_data) +{ + // get indicated airspeed from input data (raw airspeed) + _IAS = input_data.airspeed_indicated_raw; + + // to be able to detect missing data, save timestamp (used in data_missing check) + if (input_data.airspeed_timestamp != _previous_airspeed_timestamp && input_data.airspeed_timestamp > 0) { + _time_last_airspeed = input_data.timestamp; + _previous_airspeed_timestamp = input_data.airspeed_timestamp; + } + + update_EAS_scale(); + update_EAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius); + update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx, + input_data.lpos_vy, + input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q); + update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); + check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio); + check_load_factor(input_data.accel_z); + update_airspeed_valid_status(input_data.timestamp); +} + +void +AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid, + float lpos_vx, float lpos_vy, + float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4]) +{ + bool att_valid = true; // att_valid could also be a input_data state + + _wind_estimator.update(time_now_usec); + + if (lpos_valid && att_valid && _in_fixed_wing_flight) { + + Vector3f vI(lpos_vx, lpos_vy, lpos_vz); + Quatf q(att_q); + + // sideslip fusion + _wind_estimator.fuse_beta(time_now_usec, vI, q); + + // airspeed fusion (with raw TAS) + const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}}; + _wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}); + } +} + +// this function returns the current states of the wind estimator to be published in the airspeed module +wind_estimate_s +AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) +{ + wind_estimate_s wind_est = {}; + + wind_est.timestamp = timestamp; + float wind[2]; + _wind_estimator.get_wind(wind); + wind_est.windspeed_north = wind[0]; + wind_est.windspeed_east = wind[1]; + float wind_cov[2]; + _wind_estimator.get_wind_var(wind_cov); + wind_est.variance_north = wind_cov[0]; + wind_est.variance_east = wind_cov[1]; + wind_est.tas_innov = _wind_estimator.get_tas_innov(); + wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var(); + wind_est.beta_innov = _wind_estimator.get_beta_innov(); + wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var(); + wind_est.tas_scale = _wind_estimator.get_tas_scale(); + return wind_est; +} + +void +AirspeedValidator::update_EAS_scale() +{ + _EAS_scale = 1.0f / math::constrain(_wind_estimator.get_tas_scale(), 0.75f, 1.25f); +} + +void +AirspeedValidator::update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius) +{ + _EAS = calc_EAS_from_IAS(_IAS, _EAS_scale); + _TAS = calc_TAS_from_EAS(_EAS, air_pressure_pa, air_temperature_celsius); +} + +void +AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, + float estimator_status_mag_test_ratio) +{ + // Check normalised innovation levels with requirement for continuous data and use of hysteresis + // to prevent false triggering. + + if (_wind_estimator.get_wind_estimator_reset()) { + _time_wind_estimator_initialized = time_now; + } + + /* Reset states if we are not flying */ + if (!_in_fixed_wing_flight) { + // not in a flight condition that enables use of this check, thus pass check + _innovations_check_failed = false; + _time_last_tas_pass = time_now; + _time_last_tas_fail = 0; + _airspeed_valid = true; + _time_last_aspd_innov_check = time_now; + + } else { + float dt_s = math::max((time_now - _time_last_aspd_innov_check) / 1e6f, 0.01f); // limit to 100Hz + + if (dt_s < 1.0f) { + // Compute the ratio of innovation to gate size + float tas_test_ratio = _wind_estimator.get_tas_innov() * _wind_estimator.get_tas_innov() + / (fmaxf(_tas_gate, 1.0f) * fmaxf(_tas_gate, 1.0f) * _wind_estimator.get_tas_innov_var()); + + if (tas_test_ratio <= _tas_innov_threshold) { + // record pass and reset integrator used to trigger + _time_last_tas_pass = time_now; + _apsd_innov_integ_state = 0.0f; + + } else { + // integrate exceedance + _apsd_innov_integ_state += dt_s * (tas_test_ratio - _tas_innov_threshold); + } + + if ((estimator_status_vel_test_ratio < 1.0f) && (estimator_status_mag_test_ratio < 1.0f)) { + // nav velocity data is likely good so airspeed innovations are able to be used + if ((_tas_innov_integ_threshold > 0.0f) && (_apsd_innov_integ_state > _tas_innov_integ_threshold)) { + _time_last_tas_fail = time_now; + } + } + + if (!_innovations_check_failed) { + _innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY; + + } else { + _innovations_check_failed = ((time_now - _time_last_tas_fail) < TAS_INNOV_FAIL_DELAY * 100) + || (time_now - _time_wind_estimator_initialized) < TAS_INNOV_FAIL_DELAY * 100; + } + } + + _time_last_aspd_innov_check = time_now; + } +} + + +void +AirspeedValidator::check_load_factor(float accel_z) +{ + // Check if the airpeed reading is lower than physically possible given the load factor + + bool bad_number_fail = false; // disable this for now + + if (_in_fixed_wing_flight) { + + if (!bad_number_fail) { + float max_lift_ratio = fmaxf(_EAS, 0.7f) / fmaxf(_airspeed_stall, 1.0f); + max_lift_ratio *= max_lift_ratio; + + _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel_z) / 9.81f) / max_lift_ratio; + _load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f); + _load_factor_check_failed = (_load_factor_ratio > 1.1f); + + } else { + _load_factor_check_failed = true; // bad number fail + } + + } else { + + _load_factor_ratio = 0.5f; // reset if not in fixed-wing flight (and not in takeoff condition) + } + +} + + +void +AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) +{ + + bool bad_number_fail = false; // disable this for now + + // Check if sensor data is missing - assume a minimum 5Hz data rate. + const bool data_missing = (timestamp - _time_last_airspeed) > 200_ms; + + // Declare data stopped if not received for longer than 1 second + _data_stopped_failed = (timestamp - _time_last_airspeed) > 1_s; + + if (_innovations_check_failed || _load_factor_check_failed || data_missing || bad_number_fail) { + // either innovation, load factor or data missing check failed, so declare airspeed failing and record timestamp + _time_checks_failed = timestamp; + _airspeed_failing = true; + + } else if (!_innovations_check_failed && !_load_factor_check_failed && !data_missing && !bad_number_fail) { + // All checks must pass to declare airspeed good + _time_checks_passed = timestamp; + _airspeed_failing = false; + + } + + if (_airspeed_valid) { + // A simultaneous load factor and innovaton check fail makes it more likely that a large + // airspeed measurement fault has developed, so a fault should be declared immediately + const bool both_checks_failed = (_innovations_check_failed && _load_factor_check_failed); + + // Because the innovation, load factor and data missing checks are subject to short duration false positives + // a timeout period is applied. + const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s; + + if (_data_stopped_failed || both_checks_failed || single_check_fail_timeout || bad_number_fail) { + + _airspeed_valid = false; + } + + } else if ((timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) { + _airspeed_valid = true; + } +} diff --git a/src/lib/AirspeedValidator/AirspeedValidator.hpp b/src/lib/AirspeedValidator/AirspeedValidator.hpp new file mode 100644 index 000000000000..126a6efe86d3 --- /dev/null +++ b/src/lib/AirspeedValidator/AirspeedValidator.hpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * 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 AirspeedValidator.hpp + * Calculates airspeed from differential pressure and checks if this airspeed is valid. + */ + +#pragma once + +#include +#include +#include + + +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; + +using namespace time_literals; + +struct airspeed_validator_update_data { + uint64_t timestamp; + float airspeed_indicated_raw; + float airspeed_true_raw; + uint64_t airspeed_timestamp; + float lpos_vx; + float lpos_vy; + float lpos_vz; + bool lpos_valid; + float lpos_evh; + float lpos_evv; + float att_q[4]; + float air_pressure_pa; + float air_temperature_celsius; + float accel_z; + float vel_test_ratio; + float mag_test_ratio; + bool in_fixed_wing_flight; +}; + +class AirspeedValidator +{ +public: + AirspeedValidator() = default; + ~AirspeedValidator() = default; + + void update_airspeed_validator(const airspeed_validator_update_data &input_data); + + float get_IAS() { return _IAS; } + float get_EAS() { return _EAS; } + float get_TAS() { return _TAS; } + bool get_airspeed_valid() { return _airspeed_valid; } + float get_EAS_scale() {return _EAS_scale;} + + wind_estimate_s get_wind_estimator_states(uint64_t timestamp); + + // setters wind estimator parameters + void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); } + void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); } + void set_wind_estimator_tas_noise(float tas_sigma) { _wind_estimator.set_tas_noise(tas_sigma); } + void set_wind_estimator_beta_noise(float beta_var) { _wind_estimator.set_beta_noise(beta_var); } + void set_wind_estimator_tas_gate(uint8_t gate_size) + { + _tas_gate = gate_size; + _wind_estimator.set_tas_gate(gate_size); + } + + void set_wind_estimator_beta_gate(uint8_t gate_size) { _wind_estimator.set_beta_gate(gate_size); } + void set_wind_estimator_scale_estimation_on(bool scale_estimation_on) {_wind_estimator_scale_estimation_on = scale_estimation_on;} + void set_airspeed_scale(float airspeed_scale_manual) { _wind_estimator.enforce_airspeed_scale(1.0f / airspeed_scale_manual);} // scale is inverted inside the wind estimator + + // setters for failure detection tuning parameters + void set_tas_innov_threshold(float tas_innov_threshold) { _tas_innov_threshold = tas_innov_threshold; } + void set_tas_innov_integ_threshold(float tas_innov_integ_threshold) { _tas_innov_integ_threshold = tas_innov_integ_threshold; } + void set_checks_fail_delay(float checks_fail_delay) { _checks_fail_delay = checks_fail_delay; } + void set_checks_clear_delay(float checks_clear_delay) { _checks_clear_delay = checks_clear_delay; } + + void set_airspeed_stall(float airspeed_stall) { _airspeed_stall = airspeed_stall; } + +private: + + WindEstimator _wind_estimator{}; ///< wind estimator instance running in this particular airspeedValidator + + // wind estimator parameter + bool _wind_estimator_scale_estimation_on{false}; ///< online scale estimation (IAS-->CAS/EAS) is on + + // general states + bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks + float _IAS{0.0f}; ///< indicated airsped in m/s + float _EAS{0.0f}; ///< equivalent airspeed in m/s + float _TAS{0.0f}; ///< true airspeed in m/s + float _EAS_scale{1.0f}; ///< scale factor from IAS to EAS + + uint64_t _time_last_airspeed{0}; ///< time last airspeed measurement was received (uSec) + + // states of data stopped check + bool _data_stopped_failed{false}; ///< data_stopp check has detected failure + hrt_abstime _previous_airspeed_timestamp{0}; ///< timestamp from previous measurement input, to check validity of measurement + + // states of innovation check + float _tas_gate{1.0f}; ///< gate size of airspeed innovation (to calculate tas_test_ratio) + bool _innovations_check_failed{false}; ///< true when airspeed innovations have failed consistency checks + float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure + float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure + uint64_t _time_last_aspd_innov_check{0}; ///< time airspeed innovation was last checked (uSec) + uint64_t _time_last_tas_pass{0}; ///< last time innovation checks passed + uint64_t _time_last_tas_fail{0}; ///< last time innovation checks failed + float _apsd_innov_integ_state{0.0f}; ///< inegral of excess normalised airspeed innovation (sec) + static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s}; ///< time required for innovation levels to pass or fail (usec) + uint64_t _time_wind_estimator_initialized{0}; ///< time last time wind estimator was initialized (uSec) + + // states of load factor check + bool _load_factor_check_failed{false}; ///< load_factor check has detected failure + float _airspeed_stall{8.0f}; ///< stall speed of aircraft used for load factor check + float _load_factor_ratio{0.5f}; ///< ratio of maximum load factor predicted by stall speed to measured load factor + + // states of airspeed valid declaration + bool _airspeed_valid{false}; ///< airspeed valid (pitot or groundspeed-windspeed) + int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec) + int _checks_clear_delay{3}; ///< delay for airspeed valid declaration after all checks passed again (Sec) + bool _airspeed_failing{false}; ///< airspeed sensor checks have detected failure, but not yet declared failed + uint64_t _time_checks_passed{0}; ///< time the checks have last passed (uSec) + uint64_t _time_checks_failed{0}; ///< time the checks have last not passed (uSec) + + void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; } + + void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx, + float lpos_vy, + float lpos_vz, + float lpos_evh, float lpos_evv, const float att_q[4]); + void update_EAS_scale(); + void update_EAS_TAS(float air_pressure_pa, float air_temperature_celsius); + void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, + float estimator_status_mag_test_ratio); + void check_load_factor(float accel_z); + void update_airspeed_valid_status(const uint64_t timestamp); + +}; diff --git a/src/lib/AirspeedValidator/CMakeLists.txt b/src/lib/AirspeedValidator/CMakeLists.txt new file mode 100644 index 000000000000..5d015e2bcbae --- /dev/null +++ b/src/lib/AirspeedValidator/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(AirspeedValidator AirspeedValidator.cpp) + +target_include_directories(AirspeedValidator + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(AirspeedValidator PUBLIC ecl_airdata) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index c2d73adbee06..5da80dbf9012 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_git_submodule(TARGET git_ecl PATH "ecl") px4_add_git_submodule(TARGET git_matrix PATH "matrix") add_subdirectory(airspeed) +add_subdirectory(AirspeedValidator) add_subdirectory(battery) add_subdirectory(bezier) add_subdirectory(cdev) @@ -50,9 +51,10 @@ add_subdirectory(landing_slope) add_subdirectory(led) add_subdirectory(mathlib) add_subdirectory(mixer) +add_subdirectory(mixer_module) +add_subdirectory(output_limit) add_subdirectory(perf) add_subdirectory(pid) -add_subdirectory(pwm_limit) add_subdirectory(rc) add_subdirectory(systemlib) add_subdirectory(terrain_estimation) diff --git a/src/lib/CollisionPrevention/CMakeLists.txt b/src/lib/CollisionPrevention/CMakeLists.txt index ae36bf94b623..69c5bf98313c 100644 --- a/src/lib/CollisionPrevention/CMakeLists.txt +++ b/src/lib/CollisionPrevention/CMakeLists.txt @@ -33,3 +33,5 @@ px4_add_library(CollisionPrevention CollisionPrevention.cpp) target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable + +px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention ) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index a7fffebe3c2b..a3d2aa8918f0 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -38,13 +38,51 @@ */ #include + +#include + using namespace matrix; using namespace time_literals; +namespace +{ +static const int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly +static const int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG; + +float wrap_360(float f) +{ + return wrap(f, 0.f, 360.f); +} + +int wrap_bin(int i) +{ + i = i % INTERNAL_MAP_USED_BINS; + + while (i < 0) { + i += INTERNAL_MAP_USED_BINS; + } + return i; +} +} CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { + static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5"); + static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly"); + //initialize internal obstacle map + _obstacle_map_body_frame.timestamp = getTime(); + _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG; + _obstacle_map_body_frame.min_distance = UINT16_MAX; + _obstacle_map_body_frame.max_distance = 0; + _obstacle_map_body_frame.angle_offset = 0.f; + uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); + uint64_t current_time = getTime(); + + for (uint32_t i = 0 ; i < internal_bins; i++) { + _data_timestamps[i] = current_time; + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } } CollisionPrevention::~CollisionPrevention() @@ -55,213 +93,223 @@ CollisionPrevention::~CollisionPrevention() } } -void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint, - const Vector2f &adapted_setpoint) +hrt_abstime CollisionPrevention::getTime() { - collision_constraints_s constraints{}; /**< collision constraints message */ + return hrt_absolute_time(); +} - //fill in values - constraints.timestamp = hrt_absolute_time(); +hrt_abstime CollisionPrevention::getElapsedTime(const hrt_abstime *ptr) +{ + return hrt_absolute_time() - *ptr; +} - constraints.original_setpoint[0] = original_setpoint(0); - constraints.original_setpoint[1] = original_setpoint(1); - constraints.adapted_setpoint[0] = adapted_setpoint(0); - constraints.adapted_setpoint[1] = adapted_setpoint(1); +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, + const matrix::Quatf &vehicle_attitude) +{ + int msg_index = 0; + float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); + float increment_factor = 1.f / obstacle.increment; + + + if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { + //Obstacle message arrives in local_origin frame (north aligned) + //corresponding data index (convert to world frame and shift by msg offset) + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; + msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); + + //add all data points inside to FOV + if (obstacle.distances[msg_index] != UINT16_MAX) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + } - // publish constraints - if (_constraints_pub != nullptr) { - orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints); + } - } else { - _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); - } -} + } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) { + //Obstacle message arrives in body frame (front aligned) + //corresponding data index (shift by msg offset) + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { + float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + + _obstacle_map_body_frame.angle_offset; + msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); + + //add all data points inside to FOV + if (obstacle.distances[msg_index] != UINT16_MAX) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + } -void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle) -{ - // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - if (_obstacle_distance_pub != nullptr) { - orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle); + } } else { - _obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle); + mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n", + (double)obstacle.frame); } } -void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle) +void CollisionPrevention::_updateObstacleMap() { - _sub_obstacle_distance.update(); - const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); - - // Update with offboard data if the data is not stale - if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { - obstacle = obstacle_distance; - } -} + _sub_vehicle_attitude.update(); -void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) -{ + // add distance sensor data for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - distance_sensor_s distance_sensor; - _sub_distance_sensor[i].copy(&distance_sensor); - - // consider only instaces with updated, valid data and orientations useful for collision prevention - if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { - - - if (obstacle.increment > 0) { - // data from companion - obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp); - obstacle.max_distance = math::max((int)obstacle.max_distance, - (int)distance_sensor.max_distance * 100); - obstacle.min_distance = math::min((int)obstacle.min_distance, - (int)distance_sensor.min_distance * 100); - // since the data from the companion are already in the distances data structure, - // keep the increment that is sent - obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update) - - } else { - obstacle.timestamp = distance_sensor.timestamp; - obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm - obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm - memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances)); - obstacle.increment = math::degrees(distance_sensor.h_fov); - obstacle.angle_offset = 0.f; + + //if a new distance sensor message has arrived + if (_sub_distance_sensor[i].updated()) { + distance_sensor_s distance_sensor {}; + _sub_distance_sensor[i].copy(&distance_sensor); + + // consider only instances with valid data and orientations useful for collision prevention + if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { + + //update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); + _obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance, + (int)distance_sensor.max_distance * 100); + _obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance, + (int)distance_sensor.min_distance * 100); + + _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); } + } + } - if ((distance_sensor.current_distance > distance_sensor.min_distance) && - (distance_sensor.current_distance < distance_sensor.max_distance)) { + // add obstacle distance data + if (_sub_obstacle_distance.update()) { + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + + // Update map with obstacle data if the data is not stale + if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { + //update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); + _obstacle_map_body_frame.max_distance = math::max((int)_obstacle_map_body_frame.max_distance, + (int)obstacle_distance.max_distance); + _obstacle_map_body_frame.min_distance = math::min((int)_obstacle_map_body_frame.min_distance, + (int)obstacle_distance.min_distance); + _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); + } + } - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + _obstacle_distance_pub.publish(_obstacle_map_body_frame); +} - matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - // convert the sensor orientation from body to local frame in the range [0, 360] - float sensor_yaw_local_deg = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad)); +void CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, + const matrix::Quatf &vehicle_attitude) +{ + //clamp at maximum sensor range + float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance); - // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_local_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / - obstacle.increment); - int upper_bound = (int)floor((sensor_yaw_local_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / - obstacle.increment); + //discard values below min range + if ((distance_reading > distance_sensor.min_distance)) { - // if increment is lower than 5deg, use an offset - const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); + float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); - if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size - || upper_bound >= distances_array_size)) && obstacle.increment < 5.f) { - obstacle.angle_offset = sensor_yaw_local_deg ; - upper_bound = abs(upper_bound - lower_bound); - lower_bound = 0; - } + // calculate the field of view boundary bin indices + int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / + INTERNAL_MAP_INCREMENT_DEG); + int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / + INTERNAL_MAP_INCREMENT_DEG); - // rotate vehicle attitude into the sensor body frame - matrix::Quatf attitude_sensor_frame = attitude; - attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta()); + //floor values above zero, ceil values below zero + if (lower_bound < 0) { lower_bound++; } - for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrap_bin = bin; + if (upper_bound < 0) { upper_bound++; } - if (wrap_bin < 0) { - // wrap bin index around the array - wrap_bin = (int)floor(360.f / obstacle.increment) + bin; - } + // rotate vehicle attitude into the sensor body frame + matrix::Quatf attitude_sensor_frame = vehicle_attitude; + attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); + float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); - if (wrap_bin >= distances_array_size) { - // wrap bin index around the array - wrap_bin = bin - distances_array_size; - } + for (int bin = lower_bound; bin <= upper_bound; ++bin) { + int wrapped_bin = wrap_bin(bin); - // compensate measurement for vehicle tilt and convert to cm - obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], - (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch)); - } - } + // compensate measurement for vehicle tilt and convert to cm + _obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading * sensor_dist_scale); + _data_timestamps[wrapped_bin] = _obstacle_map_body_frame.timestamp; } } - - _publishObstacleDistance(obstacle); } + void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, const Vector2f &curr_vel) { - obstacle_distance_s obstacle{}; - _updateOffboardObstacleDistance(obstacle); - _updateDistanceSensor(obstacle); + _updateObstacleMap(); + + //read parameters + float col_prev_d = _param_mpc_col_prev_d.get(); + float col_prev_dly = _param_mpc_col_prev_dly.get(); + float col_prev_ang_rad = math::radians(_param_mpc_col_prev_ang.get()); + float xy_p = _param_mpc_xy_p.get(); + float max_jerk = _param_mpc_jerk_max.get(); + float max_accel = _param_mpc_acc_hor.get(); + matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - //The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms. - //that way the root does not have to be calculated for every range bin but once at the end. float setpoint_length = setpoint.norm(); - Vector2f setpoint_sqrd = setpoint * setpoint_length; - //Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees) - float max_slide_angle_rad = 0.5f; + hrt_abstime constrain_time = getTime(); - if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) { + if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { - int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); + Vector2f setpoint_dir = setpoint / setpoint_length; + float vel_max = setpoint_length; + float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d); - for (int i = 0; i < distances_array_size; i++) { + float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; + float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); - if (obstacle.distances[i] < obstacle.max_distance && - obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) { - float distance = obstacle.distances[i] / 100.0f; //convert to meters - float angle = math::radians((float)i * obstacle.increment); + for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { //disregard unused bins at the end of the message - if (obstacle.angle_offset > 0.f) { - angle += math::radians(obstacle.angle_offset); - } + //delete stale values + hrt_abstime data_age = constrain_time - _data_timestamps[i]; - //split current setpoint into parallel and orthogonal components with respect to the current bin direction - Vector2f bin_direction = {cos(angle), sin(angle)}; - Vector2f orth_direction = {-bin_direction(1), bin_direction(0)}; - float sp_parallel = setpoint_sqrd.dot(bin_direction); - float sp_orth = setpoint_sqrd.dot(orth_direction); - float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - - //calculate max allowed velocity with a P-controller (same gain as in the position controller) - float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get(); - float vel_max_posctrl = math::max(0.f, - _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance)); - float vel_max_sqrd = vel_max_posctrl * vel_max_posctrl; - - //limit the setpoint to respect vel_max by subtracting from the parallel component - if (sp_parallel > vel_max_sqrd) { - Vector2f setpoint_temp = setpoint_sqrd - (sp_parallel - vel_max_sqrd) * bin_direction; - float setpoint_temp_length = setpoint_temp.norm(); - - //limit sliding angle - float angle_diff_temp_orig = acos(setpoint_temp.dot(setpoint) / (setpoint_temp_length * setpoint_length)); - float angle_diff_temp_bin = acos(setpoint_temp.dot(bin_direction) / setpoint_temp_length); - - if (angle_diff_temp_orig > max_slide_angle_rad && setpoint_temp_length > 0.001f) { - float angle_temp_bin_cropped = angle_diff_temp_bin - (angle_diff_temp_orig - max_slide_angle_rad); - float orth_len = vel_max_sqrd * tan(angle_temp_bin_cropped); - - if (sp_orth > 0) { - setpoint_temp = vel_max_sqrd * bin_direction + orth_len * orth_direction; - - } else { - setpoint_temp = vel_max_sqrd * bin_direction - orth_len * orth_direction; - } - } + if (data_age > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + float distance = _obstacle_map_body_frame.distances[i] * 0.01f; //convert to meters + float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + + // convert from body to local frame in the range [0, 2*pi] + angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + + //get direction of current bin + Vector2f bin_direction = {cos(angle), sin(angle)}; + + if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance + && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { - setpoint_sqrd = setpoint_temp; + if (setpoint_dir.dot(bin_direction) > 0 + && setpoint_dir.dot(bin_direction) > cosf(col_prev_ang_rad)) { + //calculate max allowed velocity with a P-controller (same gain as in the position controller) + float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * (col_prev_dly + data_age * 1e-6f); + float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); + float vel_max_posctrl = xy_p * stop_distance; + float vel_max_smooth = trajmath::computeMaxSpeedFromBrakingDistance(max_jerk, max_accel, stop_distance); + Vector2f vel_max_vec = bin_direction * math::min(vel_max_posctrl, vel_max_smooth); + float vel_max_bin = vel_max_vec.dot(setpoint_dir); + + //constrain the velocity + if (vel_max_bin >= 0) { + vel_max = math::min(vel_max, vel_max_bin); + } } + + } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { + vel_max = 0.f; } } - //take the squared root - if (setpoint_sqrd.norm() > 0.001f) { - setpoint = setpoint_sqrd / std::sqrt(setpoint_sqrd.norm()); - - } else { - setpoint.zero(); - } + setpoint = setpoint_dir * vel_max; } } else { @@ -289,13 +337,21 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } _interfering = currently_interfering; - _publishConstrainedSetpoint(original_setpoint, new_setpoint); + + // publish constraints + collision_constraints_s constraints{}; + constraints.timestamp = hrt_absolute_time(); + original_setpoint.copyTo(constraints.original_setpoint); + new_setpoint.copyTo(constraints.adapted_setpoint); + _constraints_pub.publish(constraints); + original_setpoint = new_setpoint; } void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; + command.timestamp = hrt_absolute_time(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; command.param1 = (float)1; // base mode command.param3 = (float)0; // sub mode @@ -309,11 +365,5 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; // publish the vehicle command - if (_pub_vehicle_command == nullptr) { - _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, - vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); - } + _vehicle_command_pub.publish(command); } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index bd17c45020e9..31a0efaa485e 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -41,27 +41,30 @@ #pragma once -#include #include + +#include +#include +#include #include +#include +#include +#include +#include +#include +#include #include +#include #include -#include #include #include -#include -#include -#include -#include -#include -#include class CollisionPrevention : public ModuleParams { public: CollisionPrevention(ModuleParams *parent); - ~CollisionPrevention(); + virtual ~CollisionPrevention(); /** * Returs true if Collision Prevention is running */ @@ -77,14 +80,32 @@ class CollisionPrevention : public ModuleParams void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); +protected: + + obstacle_distance_s _obstacle_map_body_frame {}; + uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; + + void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude); + + /** + * Updates obstacle distance message with measurement from offboard + * @param obstacle, obstacle_distance message to be updated + */ + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + + virtual hrt_abstime getTime(); + virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr); + + private: bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ - orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ - orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */ + + uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */ @@ -94,8 +115,11 @@ class CollisionPrevention : public ModuleParams DEFINE_PARAMETERS( (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_mpc_col_prev_ang, /**< collision prevention detection angle */ (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ + (ParamFloat) _param_mpc_col_prev_dly, /**< delay of the range measurement data*/ + (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ + (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ ) /** @@ -129,6 +153,8 @@ class CollisionPrevention : public ModuleParams return offset; } + + /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention @@ -137,27 +163,25 @@ class CollisionPrevention : public ModuleParams */ void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + /** * Publishes collision_constraints message * @param original_setpoint, setpoint before collision prevention intervention * @param adapted_setpoint, collision prevention adaped setpoint */ void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + /** * Publishes obstacle_distance message with fused data from offboard and from distance sensors * @param obstacle, obstacle_distance message to be publsihed */ void _publishObstacleDistance(obstacle_distance_s &obstacle); + /** - * Updates obstacle distance message with measurement from offboard - * @param obstacle, obstacle_distance message to be updated - */ - void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle); - /** - * Updates obstacle distance message with measurement from distance sensors - * @param obstacle, obstacle_distance message to be updated + * Aggregates the sensor data into a internal obstacle map in body frame */ - void _updateDistanceSensor(obstacle_distance_s &obstacle); + void _updateObstacleMap(); + /** * Publishes vehicle command. */ diff --git a/src/lib/CollisionPrevention/CollisionPreventionTest.cpp b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp new file mode 100644 index 000000000000..31a95824e593 --- /dev/null +++ b/src/lib/CollisionPrevention/CollisionPreventionTest.cpp @@ -0,0 +1,806 @@ +/**************************************************************************** + * + * 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 + +// to run: make tests TESTFILTER=CollisionPrevention +hrt_abstime mocked_time = 0; + +class CollisionPreventionTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_reset_all(); + } +}; + +class TestCollisionPrevention : public CollisionPrevention +{ +public: + TestCollisionPrevention() : CollisionPrevention(nullptr) {} + void paramsChanged() {CollisionPrevention::updateParamsImpl();} + obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;} + void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude) + { + _addDistanceSensorData(distance_sensor, attitude); + } + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude) + { + _addObstacleSensorData(obstacle, attitude); + } +}; + +class TestTimingCollisionPrevention : public TestCollisionPrevention +{ +public: + TestTimingCollisionPrevention() : TestCollisionPrevention() {} +protected: + hrt_abstime getTime() override + { + return mocked_time; + } + + hrt_abstime getElapsedTime(const hrt_abstime *ptr) override + { + return mocked_time - *ptr; + } +}; + +TEST_F(CollisionPreventionTest, instantiation) { CollisionPrevention cp(nullptr); } + +TEST_F(CollisionPreventionTest, behaviorOff) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + + // THEN: the collision prevention should be turned off by default + EXPECT_FALSE(cp.is_active()); +} + +TEST_F(CollisionPreventionTest, noSensorData) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3.f; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + + // WHEN: we set the parameter check then apply the setpoint modification + float value = 10; // try to keep 10m away from obstacles + param_set(param, &value); + cp.paramsChanged(); + + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + // THEN: collision prevention should be enabled and limit the speed to zero + EXPECT_TRUE(cp.is_active()); + EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()); +} + +TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + matrix::Vector2f original_setpoint1(10, 0); + matrix::Vector2f original_setpoint2(-10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + vehicle_attitude_s attitude; + attitude.timestamp = hrt_absolute_time(); + attitude.q[0] = 1.0f; + attitude.q[1] = 0.0f; + attitude.q[2] = 0.0f; + attitude.q[3] = 0.0f; + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 10; // try to keep 10m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + message.min_distance = 100; + message.max_distance = 10000; + message.angle_offset = 0; + message.timestamp = hrt_absolute_time(); + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + + for (int i = 0; i < distances_array_size; i++) { + if (i < 10) { + message.distances[i] = 101; + + } else { + message.distances[i] = 10001; + } + + } + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + matrix::Vector2f modified_setpoint1 = original_setpoint1; + matrix::Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + orb_unadvertise(obstacle_distance_pub); + orb_unadvertise(vehicle_attitude_pub); + + // THEN: the internal map should know the obstacle + // case 1: the velocity setpoint should be cut down to zero + // case 2: the velocity setpoint should stay the same as the input + EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); + EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + + EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); + EXPECT_EQ(original_setpoint2, modified_setpoint2); +} + +TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + matrix::Vector2f original_setpoint1(10, 0); + matrix::Vector2f original_setpoint2(-10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + vehicle_attitude_s attitude; + attitude.timestamp = hrt_absolute_time(); + attitude.q[0] = 1.0f; + attitude.q[1] = 0.0f; + attitude.q[2] = 0.0f; + attitude.q[3] = 0.0f; + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 10; // try to keep 10m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + distance_sensor_s message; + message.timestamp = hrt_absolute_time(); + message.min_distance = 1.f; + message.max_distance = 100.f; + message.current_distance = 1.1f; + + message.variance = 0.1f; + message.signal_quality = 100; + message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + message.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + message.h_fov = math::radians(50.f); + message.v_fov = math::radians(30.f); + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &message); + orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); + orb_publish(ORB_ID(distance_sensor), distance_sensor_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + + //WHEN: We run the setpoint modification + matrix::Vector2f modified_setpoint1 = original_setpoint1; + matrix::Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + orb_unadvertise(distance_sensor_pub); + orb_unadvertise(vehicle_attitude_pub); + + // THEN: the internal map should know the obstacle + // case 1: the velocity setpoint should be cut down to zero because there is an obstacle + // case 2: the velocity setpoint should be cut down to zero because there is no data + EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); + EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + + EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); + EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1); +} + +TEST_F(CollisionPreventionTest, testPurgeOldData) +{ + // GIVEN: a simple setup condition + hrt_abstime start_time = hrt_absolute_time(); + mocked_time = start_time; + TestTimingCollisionPrevention cp; + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + vehicle_attitude_s attitude; + attitude.timestamp = start_time; + attitude.q[0] = 1.0f; + attitude.q[1] = 0.0f; + attitude.q[2] = 0.0f; + attitude.q[3] = 0.0f; + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 10; // try to keep 10m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + obstacle_distance_s message, message_empty; + memset(&message, 0xDEAD, sizeof(message)); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + message.min_distance = 100; + message.max_distance = 10000; + message.angle_offset = 0; + message.timestamp = start_time; + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + message_empty = message; + + for (int i = 0; i < distances_array_size; i++) { + if (i < 10) { + message.distances[i] = 10001; + + } else { + message.distances[i] = UINT16_MAX; + } + + message_empty.distances[i] = UINT16_MAX; + } + + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + + for (int i = 0; i < 10; i++) { + + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + mocked_time = mocked_time + 100000; //advance time by 0.1 seconds + message_empty.timestamp = mocked_time; + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_empty); + + if (i < 6) { + // THEN: If the data is new enough, the velocity setpoint should stay the same as the input + EXPECT_EQ(original_setpoint, modified_setpoint); + + } else { + // THEN: If the data is expired, the velocity setpoint should be cut down to zero because there is no data + EXPECT_FLOAT_EQ(0.f, modified_setpoint.norm()) << modified_setpoint(0) << "," << modified_setpoint(1); + } + } + + orb_unadvertise(obstacle_distance_pub); + orb_unadvertise(vehicle_attitude_pub); +} + +TEST_F(CollisionPreventionTest, noBias) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 5; // try to keep 5m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.min_distance = 100; + message.max_distance = 2000; + message.timestamp = hrt_absolute_time(); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + + for (int i = 0; i < distances_array_size; i++) { + message.distances[i] = 700; + } + + // WHEN: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + matrix::Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + orb_unadvertise(obstacle_distance_pub); + + // THEN: setpoint should go into the same direction as the stick input + EXPECT_FLOAT_EQ(original_setpoint.normalized()(0), modified_setpoint.normalized()(0)); + EXPECT_FLOAT_EQ(original_setpoint.normalized()(1), modified_setpoint.normalized()(1)); +} + +TEST_F(CollisionPreventionTest, outsideFOV) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 5; // try to keep 5m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + message.min_distance = 100; + message.max_distance = 2000; + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360.f / distances_array_size; + + //fov from 45deg to 225deg + for (int i = 0; i < distances_array_size; i++) { + float angle = i * message.increment; + + if (angle > 45.f && angle < 225.f) { + message.distances[i] = 700; + + } else { + message.distances[i] = UINT16_MAX; + } + } + + // WHEN: we publish the message and modify the setpoint for different demanded setpoints + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + + for (int i = 0; i < distances_array_size; i++) { + + float angle_deg = (float)i * message.increment; + float angle_rad = math::radians(angle_deg); + matrix::Vector2f original_setpoint = {10.f *(float)cos(angle_rad), 10.f *(float)sin(angle_rad)}; + matrix::Vector2f modified_setpoint = original_setpoint; + message.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + + if (angle_deg > 50.f && angle_deg < 230.f) { + // THEN: inside the FOV the setpoint should be limited + EXPECT_GT(modified_setpoint.norm(), 0.f); + EXPECT_LT(modified_setpoint.norm(), 10.f); + + } else { + // THEN: outside the FOV the setpoint should be clamped to zero + EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); + } + + } + + orb_unadvertise(obstacle_distance_pub); +} + +TEST_F(CollisionPreventionTest, jerkLimit) +{ + // GIVEN: a simple setup condition + TestCollisionPrevention cp; + matrix::Vector2f original_setpoint(10, 0); + float max_speed = 3; + matrix::Vector2f curr_pos(0, 0); + matrix::Vector2f curr_vel(2, 0); + + // AND: distance set to 5m + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + float value = 5; // try to keep 5m distance + param_set(param, &value); + cp.paramsChanged(); + + // AND: an obstacle message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.min_distance = 100; + message.max_distance = 2000; + message.timestamp = hrt_absolute_time(); + message.frame = message.MAV_FRAME_GLOBAL; //north aligned + int distances_array_size = sizeof(message.distances) / sizeof(message.distances[0]); + message.increment = 360 / distances_array_size; + + for (int i = 0; i < distances_array_size; i++) { + message.distances[i] = 700; + } + + // AND: we publish the message and set the parameter and then run the setpoint modification + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); + orb_unadvertise(obstacle_distance_pub); + + // AND: we now set max jerk to 0.1 + param = param_handle(px4::params::MPC_JERK_MAX); + value = 0.1; // 0.1 maximum jerk + param_set(param, &value); + cp.paramsChanged(); + + // WHEN: we run the setpoint modification again + matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel); + + // THEN: the new setpoint should be much slower than the one with default jerk + EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm()); +} + +TEST_F(CollisionPreventionTest, addDistanceSensorData) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + cp.getObstacleMap().increment = 10.f; + matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (uint32_t i = 0; i < distances_array_size; i++) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //WHEN: we add distance sensor data to the right + distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; + distance_sensor.h_fov = math::radians(19.99f); + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 8 || i == 9) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } + + //WHEN: we add additionally distance sensor data to the left + distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + distance_sensor.h_fov = math::radians(50.f); + distance_sensor.current_distance = 8.f; + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 8 || i == 9) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else if (i >= 24 && i <= 29) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } + + //WHEN: we add additionally distance sensor data to the front + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(10.1f); + distance_sensor.current_distance = 3.f; + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 8 || i == 9) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else if (i >= 24 && i <= 29) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + + } else if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } + + +} + +TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + cp.getObstacleMap().increment = 10.f; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned + obstacle_msg.increment = 5.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); + matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); + matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + matrix::Euler attitude4_euler(0, 0, M_PI); + matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + + //obstacle at 10-30 deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 2; i < 6 ; i++) { + obstacle_msg.distances[i] = 500; + } + + + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //WHEN: we add distance sensor data while vehicle has zero yaw + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 1 || i == 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + + //WHEN: we add distance sensor data while vehicle yaw 90deg to the right + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 28 || i == 29) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add distance sensor data while vehicle yaw 45deg to the left + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 6 || i == 7) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add distance sensor data while vehicle yaw 180deg + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 19 || i == 20) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} + +TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + cp.getObstacleMap().increment = 10.f; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned + obstacle_msg.increment = 5.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform + matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); + matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw + matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); + matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw + matrix::Euler attitude4_euler(0, 0, M_PI); + matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw + + //obstacle at 10-30 deg body frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 2; i < 6 ; i++) { + obstacle_msg.distances[i] = 500; + } + + + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //WHEN: we add obstacle data while vehicle has zero yaw + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 1 || i == 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add obstacle data while vehicle yaw 90deg to the right + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 1 || i == 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add obstacle data while vehicle yaw 45deg to the left + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 1 || i == 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add obstacle data while vehicle yaw 180deg + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i == 1 || i == 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} + + +TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + cp.getObstacleMap().increment = 10.f; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned + obstacle_msg.increment = 6.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + + //obstacle at 0-30 deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 0; i < 5 ; i++) { + obstacle_msg.distances[i] = 500; + } + + //WHEN: we add distance sensor data + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 0 && i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add distance sensor data with an angle offset + obstacle_msg.angle_offset = 30.f; + cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { + if (i >= 3 && i <= 5) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c index a69a9ccc95c4..664e4c05ccb4 100644 --- a/src/lib/CollisionPrevention/collisionprevention_params.c +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -62,3 +62,15 @@ PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_COL_PREV_DLY, 0.f); + +/** + * Angle left/right from the commanded setpoint in which the range data is used to calculate speed limitations. All data further from the commanded direction is not considered. + * + * Only used in Position mode. + * + * @min 0 + * @max 90 + * @unit [deg] + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_ANG, 45.f); diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 06277ef49fb8..c7d2fe2d4f59 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 06277ef49fb8c9fad18d56ef40e8bc9fe1655a65 +Subproject commit c7d2fe2d4f5954b9042af4edf7e5aeb21dbdd129 diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index cdc4aa16d2f7..7e1a30741ffa 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -57,11 +57,11 @@ const landing_gear_s FlightTasks::getGear() } } -int FlightTasks::switchTask(FlightTaskIndex new_task_index) +FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index) { // switch to the running task, nothing to do if (new_task_index == _current_task.index) { - return 0; + return FlightTaskError::NoError; } // Save current setpoints for the nex FlightTask @@ -69,12 +69,12 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) if (_initTask(new_task_index)) { // invalid task - return -1; + return FlightTaskError::InvalidTask; } if (!_current_task.task) { // no task running - return 0; + return FlightTaskError::NoError; } // subscription failed @@ -82,7 +82,7 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _current_task.task->~FlightTask(); _current_task.task = nullptr; _current_task.index = FlightTaskIndex::None; - return -2; + return FlightTaskError::SubscriptionFailed; } _subscription_array.forcedUpdate(); // make sure data is available for all new subscriptions @@ -92,13 +92,13 @@ int FlightTasks::switchTask(FlightTaskIndex new_task_index) _current_task.task->~FlightTask(); _current_task.task = nullptr; _current_task.index = FlightTaskIndex::None; - return -3; + return FlightTaskError::ActivationFailed; } - return 0; + return FlightTaskError::NoError; } -int FlightTasks::switchTask(int new_task_index) +FlightTaskError FlightTasks::switchTask(int new_task_index) { // make sure we are in range of the enumeration before casting if (static_cast(FlightTaskIndex::None) <= new_task_index && @@ -107,7 +107,7 @@ int FlightTasks::switchTask(int new_task_index) } switchTask(FlightTaskIndex::None); - return -1; + return FlightTaskError::InvalidTask; } void FlightTasks::handleParameterUpdate() @@ -117,10 +117,10 @@ void FlightTasks::handleParameterUpdate() } } -const char *FlightTasks::errorToString(const int error) +const char *FlightTasks::errorToString(const FlightTaskError error) { for (auto e : _taskError) { - if (e.error == error) { + if (static_cast(e.error) == error) { return e.msg; } } @@ -137,22 +137,16 @@ void FlightTasks::reActivate() void FlightTasks::_updateCommand() { - // lazy subscription to command topic - if (_sub_vehicle_command < 0) { - _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - } - // check if there's any new command - bool updated = false; - orb_check(_sub_vehicle_command, &updated); + bool updated = _sub_vehicle_command.updated(); if (!updated) { return; } // get command - struct vehicle_command_s command; - orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); + vehicle_command_s command{}; + _sub_vehicle_command.copy(&command); // check what command it is FlightTaskIndex desired_task = switchVehicleCommand(command.command); @@ -163,11 +157,11 @@ void FlightTasks::_updateCommand() } // switch to the commanded task - int switch_result = switchTask(desired_task); + FlightTaskError switch_result = switchTask(desired_task); uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; // if we are in/switched to the desired task - if (switch_result >= 0) { + if (switch_result >= FlightTaskError::NoError) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; // if the task is running apply parameters to it and see if it rejects @@ -175,7 +169,7 @@ void FlightTasks::_updateCommand() cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; // if we just switched and parameters are not accepted, go to failsafe - if (switch_result == 1) { + if (switch_result >= FlightTaskError::NoError) { switchTask(FlightTaskIndex::ManualPosition); cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; } @@ -183,19 +177,13 @@ void FlightTasks::_updateCommand() } // send back acknowledgment - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = command.command; command_ack.result = cmd_result; - command_ack.result_param1 = switch_result; + command_ack.result_param1 = static_cast(switch_result); command_ack.target_system = command.source_system; command_ack.target_component = command.source_component; - if (_pub_vehicle_command_ack == nullptr) { - _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); - - } + _pub_vehicle_command_ack.publish(command_ack); } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index dc5020c2b2c7..c6a401fc2737 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -45,9 +45,20 @@ #include "SubscriptionArray.hpp" #include "FlightTasks_generated.hpp" #include +#include +#include +#include +#include #include +enum class FlightTaskError : int { + NoError = 0, + InvalidTask = -1, + SubscriptionFailed = -2, + ActivationFailed = -3 +}; + class FlightTasks { public: @@ -98,17 +109,17 @@ class FlightTasks /** * Switch to the next task in the available list (for testing) - * @return 1 on success, <0 on error + * @return 0 on success, <0 on error */ - int switchTask() { return switchTask(static_cast(_current_task.index) + 1); } + FlightTaskError switchTask() { return switchTask(static_cast(_current_task.index) + 1); } /** * Switch to a specific task (for normal usage) * @param task index to switch to - * @return 1 on success, 0 on no change, <0 on error + * @return 0 on success, <0 on error */ - int switchTask(FlightTaskIndex new_task_index); - int switchTask(int new_task_index); + FlightTaskError switchTask(FlightTaskIndex new_task_index); + FlightTaskError switchTask(int new_task_index); /** * Get the number of the active task @@ -130,7 +141,7 @@ class FlightTasks /** * Call this method to get the description of a task error. */ - const char *errorToString(const int error); + const char *errorToString(const FlightTaskError error); /** * Sets an external yaw handler. The active flight task can use the yaw handler to implement a different yaw control strategy. @@ -170,18 +181,20 @@ class FlightTasks * Map from Error int to user friendly string. */ task_error_t _taskError[_numError] = { - {0, "No Error"}, - {-1, "Invalid Task "}, - {-2, "Subscription Failed"}, - {-3, "Activation Failed"} + {static_cast(FlightTaskError::NoError), "No Error"}, + {static_cast(FlightTaskError::InvalidTask), "Invalid Task "}, + {static_cast(FlightTaskError::SubscriptionFailed), "Subscription Failed"}, + {static_cast(FlightTaskError::ActivationFailed), "Activation Failed"} }; /** * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them */ void _updateCommand(); FlightTaskIndex switchVehicleCommand(const int command); - int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ - orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */ + + uORB::Subscription _sub_vehicle_command{ORB_ID(vehicle_command)}; /**< topic handle on which commands are received */ + + uORB::PublicationQueued _pub_vehicle_command_ack{ORB_ID(vehicle_command_ack)}; int _initTask(FlightTaskIndex task_index); }; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 8f9f3ac987e9..c46f59c0e84b 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -125,12 +125,14 @@ void FlightTaskAuto::_limitYawRate() const float dyaw_desired = matrix::wrap_pi(_yaw_setpoint - _yaw_sp_prev); const float dyaw_max = yawrate_max * _deltatime; const float dyaw = math::constrain(dyaw_desired, -dyaw_max, dyaw_max); - _yaw_setpoint = _yaw_sp_prev + dyaw; - _yaw_setpoint = matrix::wrap_pi(_yaw_setpoint); - _yaw_sp_prev = _yaw_setpoint; + float yaw_setpoint_sat = _yaw_sp_prev + dyaw; + yaw_setpoint_sat = matrix::wrap_pi(yaw_setpoint_sat); - // The yaw setpoint is aligned when its rate is not saturated - _yaw_sp_aligned = fabsf(dyaw_desired) < fabsf(dyaw_max); + // The yaw setpoint is aligned when it is within tolerance + _yaw_sp_aligned = fabsf(_yaw_setpoint - yaw_setpoint_sat) < math::radians(_param_mis_yaw_err.get()); + + _yaw_setpoint = yaw_setpoint_sat; + _yaw_sp_prev = _yaw_setpoint; } if (PX4_ISFINITE(_yawspeed_setpoint)) { @@ -257,6 +259,12 @@ bool FlightTaskAuto::_evaluateTriplets() } } + if (_ext_yaw_handler != nullptr) { + // activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane) + (_param_wv_en.get() && _sub_triplet_setpoint->get().current.allow_weather_vane) ? _ext_yaw_handler->activate() : + _ext_yaw_handler->deactivate(); + } + // set heading if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) { _yaw_setpoint = _yaw; @@ -292,9 +300,8 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_next_wp, _sub_triplet_setpoint->get().next.yaw, _sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN, - _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()); - _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt, - _sub_triplet_setpoint->get().current.type); + _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active(), _sub_triplet_setpoint->get().current.type); + _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); } return true; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index c7031da89872..c14371dbb77d 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -119,7 +119,9 @@ class FlightTaskAuto : public FlightTask _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, (ParamInt) _param_com_obs_avoid, // obstacle avoidance active - (ParamFloat) _param_mpc_yawrauto_max + (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mis_yaw_err, // yaw-error threshold + (ParamBool) _param_wv_en // enable/disable weather vane (VTOL) ); private: diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp index b62dc0086d07..3bd52991101d 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp @@ -93,7 +93,7 @@ void FlightTaskAutoLine::_generateXYsetpoints() { _setSpeedAtTarget(); Vector2f pos_sp_to_dest(_target - _position_setpoint); - const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _target_acceptance_radius; + const bool has_reached_altitude = fabsf(_target(2) - _position(2)) < _param_nav_mc_alt_rad.get(); if ((_speed_at_target < 0.001f && pos_sp_to_dest.length() < _target_acceptance_radius) || (!has_reached_altitude && pos_sp_to_dest.length() < _target_acceptance_radius)) { @@ -175,14 +175,8 @@ void FlightTaskAutoLine::_generateXYsetpoints() // Vehicle is still far from destination. Accelerate or keep maximum target speed. float acc_track = (speed_sp_track - speed_sp_prev_track) / _deltatime; - float yaw_diff = 0.0f; - - if (PX4_ISFINITE(_yaw_setpoint)) { - yaw_diff = wrap_pi(_yaw_setpoint - _yaw); - } - // If yaw offset is large, only accelerate with 0.5 m/s^2. - float acc_max = (fabsf(yaw_diff) > math::radians(_param_mis_yaw_err.get())) ? 0.5f : _param_mpc_acc_hor.get(); + float acc_max = (_yaw_sp_aligned) ? _param_mpc_acc_hor.get() : 0.5f; if (acc_track > acc_max) { // accelerate towards target diff --git a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp index 338045bdada7..94d0ad0ed410 100644 --- a/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp +++ b/src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.hpp @@ -51,7 +51,6 @@ class FlightTaskAutoLine : public FlightTaskAutoMapper protected: DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper, - (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index f538149f356b..b6a3697a7e05 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -38,6 +38,7 @@ #include "FlightTaskAutoLineSmoothVel.hpp" #include #include +#include "TrajMath.hpp" using namespace matrix; @@ -121,9 +122,12 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() { bool res = false; Vector2f vel_sp_xy(_velocity_setpoint); + Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position); - if (vel_sp_xy.length() > .1f) { + if ((vel_sp_xy.length() > .1f) && + (traj_to_target.length() > _target_acceptance_radius)) { // Generate heading from velocity vector, only if it is long enough + // and if the drone is far enough from the target _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy); res = true; } @@ -131,18 +135,23 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() return res; } -/* Constrain some value vith a constrain depending on the sign of the constrain +/* Constrain some value vith a constrain depending on the sign of the constraint * Example: - if the constrain is -5, the value will be constrained between -5 and 0 * - if the constrain is 5, the value will be constrained between 0 and 5 */ -inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constrain) +inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint) { - const float min = (constrain < FLT_EPSILON) ? constrain : 0.f; - const float max = (constrain > FLT_EPSILON) ? constrain : 0.f; + const float min = (constraint < FLT_EPSILON) ? constraint : 0.f; + const float max = (constraint > FLT_EPSILON) ? constraint : 0.f; return math::constrain(val, min, max); } +float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max) +{ + return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max)); +} + void FlightTaskAutoLineSmoothVel::_initEkfResetCounters() { _reset_counters.xy = _sub_vehicle_local_position->get().xy_reset_counter; @@ -177,6 +186,53 @@ void FlightTaskAutoLineSmoothVel::_checkEkfResetCounters() } } +float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() +{ + // Compute the maximum allowed speed at the waypoint assuming that we want to + // connect the two lines (prev-current and current-next) + // with a tangent circle with constant speed and desired centripetal acceleration: a_centripetal = speed^2 / radius + // The circle should in theory start and end at the intersection of the lines and the waypoint's acceptance radius. + // This is not exactly true in reality since Navigator switches the waypoint so we have to take in account that + // the real acceptance radius is smaller. + // It can be that the next waypoint is the last one or that the drone will have to stop for some other reason + // so we have to make sure that the speed at the current waypoint allows to stop at the next waypoint. + float speed_at_target = 0.0f; + + const float distance_current_next = Vector2f(&(_target - _next_wp)(0)).length(); + const bool waypoint_overlap = Vector2f(&(_target - _prev_wp)(0)).length() < _target_acceptance_radius; + const bool yaw_align_check_pass = (_param_mpc_yaw_mode.get() != 4) || _yaw_sp_aligned; + + if (distance_current_next > 0.001f && + !waypoint_overlap && + yaw_align_check_pass) { + // Max speed between current and next + const float max_speed_current_next = _getMaxSpeedFromDistance(distance_current_next); + const float alpha = acos(Vector2f(&(_target - _prev_wp)(0)).unit_or_zero() * + Vector2f(&(_target - _next_wp)(0)).unit_or_zero()); + // We choose a maximum centripetal acceleration of MPC_ACC_HOR * MPC_XY_TRAJ_P to take in account + // that there is a jerk limit (a direct transition from line to circle is not possible) + // MPC_XY_TRAJ_P should be between 0 and 1. + const float max_speed_in_turn = trajmath::computeMaxSpeedInWaypoint(alpha, + _param_mpc_xy_traj_p.get() * _param_mpc_acc_hor.get(), + _target_acceptance_radius); + speed_at_target = math::min(math::min(max_speed_in_turn, max_speed_current_next), _mc_cruise_speed); + } + + return speed_at_target; +} + +float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) +{ + float max_speed = trajmath::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(), + _param_mpc_acc_hor.get(), + braking_distance); + // To avoid high gain at low distance due to the sqrt, we take the minimum + // of this velocity and a slope of "traj_p" m/s per meter + max_speed = math::min(max_speed, braking_distance * _param_mpc_xy_traj_p.get()); + + return max_speed; +} + void FlightTaskAutoLineSmoothVel::_prepareSetpoints() { // Interface: A valid position setpoint generates a velocity target using a P controller. If a velocity is specified @@ -195,40 +251,48 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints() PX4_ISFINITE(_position_setpoint(1))) { // Use position setpoints to generate velocity setpoints - // Get various path specific vectors. */ - Vector2f pos_traj; + // Get various path specific vectors + Vector3f pos_traj; pos_traj(0) = _trajectory[0].getCurrentPosition(); pos_traj(1) = _trajectory[1].getCurrentPosition(); - Vector2f pos_sp_xy(_position_setpoint); - Vector2f pos_traj_to_dest(pos_sp_xy - pos_traj); - Vector2f u_prev_to_dest = Vector2f(pos_sp_xy - Vector2f(_prev_wp)).unit_or_zero(); - Vector2f prev_to_pos(pos_traj - Vector2f(_prev_wp)); - Vector2f closest_pt = Vector2f(_prev_wp) + u_prev_to_dest * (prev_to_pos * u_prev_to_dest); - Vector2f u_pos_traj_to_dest_xy(Vector2f(pos_traj_to_dest).unit_or_zero()); - - // Compute the maximum possible velocity on the track given the remaining distance, the maximum acceleration and the maximum jerk. - // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from opposite max acceleration) - // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) - // To avoid high gain at low distance due to the sqrt, we take the minimum of this velocity and a slope of "traj_p" m/s per meter - float b = 4.f * _param_mpc_acc_hor.get() * _param_mpc_acc_hor.get() / _param_mpc_jerk_auto.get(); - float c = - 2.f * _param_mpc_acc_hor.get() * pos_traj_to_dest.length(); - float max_speed = 0.5f * (-b + sqrtf(b * b - 4.f * c)); - float speed_sp_track = math::min(max_speed, pos_traj_to_dest.length() * _param_mpc_xy_traj_p.get()); - speed_sp_track = math::constrain(speed_sp_track, 0.0f, _mc_cruise_speed); - - Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * speed_sp_track; + pos_traj(2) = _trajectory[2].getCurrentPosition(); + Vector2f pos_traj_to_dest_xy(_position_setpoint - pos_traj); + Vector2f u_pos_traj_to_dest_xy(pos_traj_to_dest_xy.unit_or_zero()); + + // Unconstrained desired velocity vector + Vector2f vel_sp_xy = u_pos_traj_to_dest_xy * _mc_cruise_speed; + + Vector2f vel_max_xy; + vel_max_xy(0) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(0))); + vel_max_xy(1) = _getMaxSpeedFromDistance(fabsf(pos_traj_to_dest_xy(1))); + + const bool has_reached_altitude = fabsf(_position_setpoint(2) - pos_traj(2)) < _param_nav_mc_alt_rad.get(); + Vector2f vel_min_xy; + + if (has_reached_altitude) { + // Compute the minimum speed in NE frame. This is used + // to force the drone to pass the waypoint with a desired speed + Vector2f u_prev_to_target_xy((_target - _prev_wp).unit_or_zero()); + vel_min_xy = u_prev_to_target_xy * _getSpeedAtTarget(); + + } else { + // The drone has to change altitude, stop at the waypoint + vel_min_xy.setAll(0.f); + } + + // Constrain the norm of each component using min and max values + Vector2f vel_sp_constrained_xy; + vel_sp_constrained_xy(0) = _constrainAbs(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0)); + vel_sp_constrained_xy(1) = _constrainAbs(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1)); for (int i = 0; i < 2; i++) { // If available, constrain the velocity using _velocity_setpoint(.) if (PX4_ISFINITE(_velocity_setpoint(i))) { - _velocity_setpoint(i) = _constrainOneSide(vel_sp_xy(i), _velocity_setpoint(i)); + _velocity_setpoint(i) = _constrainOneSide(vel_sp_constrained_xy(i), _velocity_setpoint(i)); } else { - _velocity_setpoint(i) = vel_sp_xy(i); + _velocity_setpoint(i) = vel_sp_constrained_xy(i); } - - _velocity_setpoint(i) += (closest_pt(i) - _trajectory[i].getCurrentPosition()) * - _param_mpc_xy_traj_p.get(); // Along-track setpoint + cross-track P controller } } diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp index 41e90e8e9f51..06b2957473cc 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp @@ -65,12 +65,16 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2 ); void checkSetpoints(vehicle_local_position_setpoint_s &setpoints); + float _getSpeedAtTarget(); + float _getMaxSpeedFromDistance(float braking_distance); void _generateSetpoints() override; /**< Generate setpoints along line. */ /** determines when to trigger a takeoff (ignored in flight) */ bool _checkTakeoff() override { return _want_takeoff; }; - inline float _constrainOneSide(float val, float constrain); + inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */ + inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */ + void _initEkfResetCounters(); void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */ void _generateHeading(); diff --git a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp index 1387609ef2f2..173291919e33 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp @@ -87,7 +87,7 @@ bool FlightTaskAutoMapper::update() } if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint); + _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint); } diff --git a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp index 67c4c1c6f2a2..ed35fe45c9cd 100644 --- a/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp +++ b/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp @@ -92,7 +92,7 @@ bool FlightTaskAutoMapper2::update() } if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint); + _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, _yawspeed_setpoint); } diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 645ab439dde1..6de157339f26 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -157,12 +157,6 @@ class FlightTask : public ModuleParams */ static const landing_gear_s empty_landing_gear_default_keep; - /** - * Empty desired waypoints. - * All waypoints are set to NAN. - */ - static const vehicle_trajectory_waypoint_s empty_trajectory_waypoint; - /** * Call this whenever a parameter update notification is received (parameter_update uORB message) */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index a6919c07c62e..b2e5ec473b6f 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -130,7 +130,9 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() * is used to set current velocity of the trajectory. */ - if (fabsf(_sticks_expo(2)) > FLT_EPSILON) { + const float velocity_target_z = _velocity_setpoint(2); + + if (fabsf(velocity_target_z) > FLT_EPSILON) { if (_position_lock_z_active) { _smoothing.setCurrentVelocity(_velocity_setpoint_feedback( 2)); // Start the trajectory at the current velocity setpoint @@ -159,7 +161,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() if (fabsf(_vel_sp_smooth) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(_sticks_expo(2)) <= FLT_EPSILON) { + fabsf(velocity_target_z) <= FLT_EPSILON) { _position_lock_z_active = true; } diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 62ac3c1857d2..d73b012ee656 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -169,9 +169,10 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller * is used to set current velocity of the trajectory. */ - Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0)); + const Vector2f velocity_target_xy = Vector2f(_velocity_setpoint); + const float velocity_target_z = _velocity_setpoint(2); - if (sticks_expo_xy.length() > FLT_EPSILON) { + if (velocity_target_xy.length() > FLT_EPSILON) { if (_position_lock_xy_active) { _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback( 0)); // Start the trajectory at the current velocity setpoint @@ -183,7 +184,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_xy_active = false; } - if (fabsf(_sticks_expo(2)) > FLT_EPSILON) { + if (fabsf(velocity_target_z) > FLT_EPSILON) { if (_position_lock_z_active) { _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback( 2)); // Start the trajectory at the current velocity setpoint @@ -233,13 +234,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() // Check for position lock transition if (Vector2f(_vel_sp_smooth).length() < 0.1f && Vector2f(_acceleration_setpoint).length() < .2f && - sticks_expo_xy.length() <= FLT_EPSILON) { + velocity_target_xy.length() <= FLT_EPSILON) { _position_lock_xy_active = true; } if (fabsf(_vel_sp_smooth(2)) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(_sticks_expo(2)) <= FLT_EPSILON) { + fabsf(velocity_target_z) <= FLT_EPSILON) { _position_lock_z_active = true; } diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index 2c496e56b12d..f3dfc59ab2a9 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,7 +42,7 @@ using namespace matrix; -FlightTaskOrbit::FlightTaskOrbit() +FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position) { _sticks_data_required = false; } @@ -92,6 +92,9 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) } } + // perpendicularly approach the orbit circle again when new parameters get commanded + _in_circle_approach = true; + return ret; } @@ -178,8 +181,47 @@ bool FlightTaskOrbit::update() setRadius(r); setVelocity(v); - // xy velocity to go around in a circle Vector2f center_to_position = Vector2f(_position) - _center; + + // make vehicle front always point towards the center + _yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; + + if (_in_circle_approach) { + generate_circle_approach_setpoints(); + + } else { + generate_circle_setpoints(center_to_position); + } + + // publish information to UI + sendTelemetry(); + + return true; +} + +void FlightTaskOrbit::generate_circle_approach_setpoints() +{ + if (_circle_approach_line.isEndReached()) { + // calculate target point on circle and plan a line trajectory + Vector2f start_to_center = _center - Vector2f(_position); + Vector2f start_to_circle = (start_to_center.norm() - _r) * start_to_center.unit_or_zero(); + Vector2f closest_circle_point = Vector2f(_position) + start_to_circle; + Vector3f target = Vector3f(closest_circle_point(0), closest_circle_point(1), _position(2)); + _circle_approach_line.setLineFromTo(_position, target); + _circle_approach_line.setSpeed(_param_mpc_xy_cruise.get()); + } + + // follow the planned line and switch to orbiting once the circle is reached + _circle_approach_line.generateSetpoints(_position_setpoint, _velocity_setpoint); + _in_circle_approach = !_circle_approach_line.isEndReached(); + + // yaw stays constant + _yawspeed_setpoint = NAN; +} + +void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position) +{ + // xy velocity to go around in a circle Vector2f velocity_xy(-center_to_position(1), center_to_position(0)); velocity_xy = velocity_xy.unit_or_zero(); velocity_xy *= _v; @@ -189,14 +231,8 @@ bool FlightTaskOrbit::update() _velocity_setpoint(0) = velocity_xy(0); _velocity_setpoint(1) = velocity_xy(1); + _position_setpoint(0) = _position_setpoint(1) = NAN; - // make vehicle front always point towards the center - _yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F; // yawspeed feed-forward because we know the necessary angular rate _yawspeed_setpoint = _v / _r; - - // publish telemetry - sendTelemetry(); - - return true; } diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp index e7688df98f14..08df6a453f8d 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-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 @@ -43,6 +43,7 @@ #include "FlightTaskManualAltitudeSmooth.hpp" #include +#include class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth { @@ -86,10 +87,16 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth bool setVelocity(const float v); private: + void generate_circle_approach_setpoints(); /**< generates setpoints to smoothly reach the closest point on the circle when starting from far away */ + void generate_circle_setpoints(matrix::Vector2f center_to_position); /**< generates xy setpoints to orbit the vehicle */ + float _r = 0.f; /**< radius with which to orbit the target */ float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */ matrix::Vector2f _center; /**< local frame coordinates of the center point */ + bool _in_circle_approach = false; + StraightLine _circle_approach_line; + // TODO: create/use parameters for limits const float _radius_min = 1.f; const float _radius_max = 100.f; @@ -97,4 +104,8 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth const float _acceleration_max = 2.f; orb_advert_t _orbit_status_pub = nullptr; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_xy_cruise /**< cruise speed for circle approach */ + ) }; diff --git a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt index 94f6d1173a39..14cf505b344e 100644 --- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt @@ -39,5 +39,8 @@ px4_add_library(FlightTaskUtility VelocitySmoothing.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility) +px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index 2fac77484c3a..eafcd6f6ed9b 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp @@ -44,15 +44,7 @@ using namespace time_literals; static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; /** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; - -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, {0, 0, 0}} - } -}; +static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : ModuleParams(parent) @@ -60,19 +52,7 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : _desired_waypoint = empty_trajectory_waypoint; _failsafe_position.setNaN(); _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - -} - -ObstacleAvoidance::~ObstacleAvoidance() -{ - //unadvertise publishers - if (_pub_traj_wp_avoidance_desired != nullptr) { - orb_unadvertise(_pub_traj_wp_avoidance_desired); - } - - if (_pub_pos_control_status != nullptr) { - orb_unadvertise(_pub_pos_control_status); - } + _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); } @@ -139,8 +119,8 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel } void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active) + const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, + const bool ext_yaw_active, const int wp_type) { _desired_waypoint.timestamp = hrt_absolute_time(); _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; @@ -154,6 +134,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; + _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); @@ -164,19 +145,20 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; } -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp) +void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) { pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); + _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - _publishAvoidanceDesiredWaypoint(); + _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); _desired_waypoint = empty_trajectory_waypoint; } void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt, const int wp_type) + float target_acceptance_radius, const Vector2f &closest_pt) { _position = pos; position_controller_status_s pos_control_status = {}; @@ -199,36 +181,21 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); + bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); + _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); + if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + && _no_progress_z_hysteresis.get_state()) { // vehicle above or below the target waypoint pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; } + _prev_pos_to_target_z = pos_to_target_z; + // do not check for waypoints yaw acceptance in navigator pos_control_status.yaw_acceptance = NAN; - if (_pub_pos_control_status == nullptr) { - _pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status); - - } else { - orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status); - - } - -} - -void -ObstacleAvoidance::_publishAvoidanceDesiredWaypoint() -{ - // publish desired waypoint - if (_pub_traj_wp_avoidance_desired != nullptr) { - orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _pub_traj_wp_avoidance_desired, &_desired_waypoint); - - } else { - _pub_traj_wp_avoidance_desired = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired), - &_desired_waypoint); - } + _pub_pos_control_status.publish(pos_control_status); } void ObstacleAvoidance::_publishVehicleCmdDoLoiter() @@ -248,11 +215,5 @@ void ObstacleAvoidance::_publishVehicleCmdDoLoiter() command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; // publish the vehicle command - if (_pub_vehicle_command == nullptr) { - _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, - vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); - } + _pub_vehicle_command.publish(command); } diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index f873e1702b6e..241575750bd2 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -46,6 +46,8 @@ #include #include +#include +#include #include #include #include @@ -58,11 +60,20 @@ #include +const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, + { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} + } +}; + class ObstacleAvoidance : public ModuleParams { public: ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance(); + ~ObstacleAvoidance() = default; bool initializeSubscriptions(SubscriptionArray &subscription_array); @@ -83,15 +94,18 @@ class ObstacleAvoidance : public ModuleParams * @param next_wp, next position triplet * @param next_yaw, next yaw triplet * @param next_yawspeed, next yaw speed triplet + * @param wp_type, current triplet type */ void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active); + const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, + const int wp_type); /** * Updates the desired setpoints to send to the obstacle avoidance system. * @param pos_sp, desired position setpoint computed by the active FlightTask * @param vel_sp, desired velocity setpoint computed by the active FlightTask + * @param type, current triplet type */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp); + void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); /** * Checks the vehicle progress between previous and current position waypoint of the triplet. @@ -99,41 +113,39 @@ class ObstacleAvoidance : public ModuleParams * @param prev_wp, previous position triplet * @param target_acceptance_radius, current position triplet xy acceptance radius * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - * @param wp_type, current triplet type */ void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt, const int wp_type); + float target_acceptance_radius, const matrix::Vector2f &closest_pt); -private: +protected: uORB::SubscriptionPollable *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */ uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); + vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ - vehicle_trajectory_waypoint_s _desired_waypoint = {}; /**< desired vehicle trajectory waypoint to be sent to OA */ - orb_advert_t _pub_traj_wp_avoidance_desired{nullptr}; /**< trajectory waypoint desired publication */ - orb_advert_t _pub_pos_control_status{nullptr}; /**< position controller status publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */ + uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ + uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ + uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ matrix::Vector3f _curr_wp = {}; /**< current position triplet */ matrix::Vector3f _position = {}; /**< current vehicle position */ matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ + systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ + float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - /** - * Publishes vehicle trajectory waypoint desired. - */ - void _publishAvoidanceDesiredWaypoint(); + bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ /** * Publishes vehicle command. */ void _publishVehicleCmdDoLoiter(); + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ + ); + }; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp new file mode 100644 index 000000000000..9f25375ec9ae --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + + +using namespace matrix; +// to run: make tests TESTFILTER=CollisionPrevention + +class ObstacleAvoidanceTest : public ::testing::Test +{ +public: + SubscriptionArray subscription_array; + Vector3f pos_sp; + Vector3f vel_sp; + float yaw_sp = 0.123f; + float yaw_speed_sp = NAN; + void SetUp() override + + { + param_reset_all(); + pos_sp = Vector3f(1.f, 1.2f, 0.1f); + vel_sp = Vector3f(0.3f, 0.4f, 0.1f); + } +}; + +class TestObstacleAvoidance : public ::ObstacleAvoidance +{ +public: + TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} + void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} + void test_setPosition(Vector3f &pos) {_position = pos;} +}; + +TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } + +TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming + // from offboard + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; + message.timestamp = hrt_absolute_time(); + message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; + + // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message + orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); + orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + + vehicle_status_s vehicle_status{}; + vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); + orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + + subscription_array.update(); + + // WHEN: we inject the vehicle_trajectory_waypoint in the interface + oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); + + // THEN: the setpoints should be injected + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); + EXPECT_TRUE(vel_sp.isAllNan()); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); + EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); +} + +TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; + Vector3f pos(3.1f, 4.7f, 5.2f); + oa.test_setPosition(pos); + + // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status + orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); + orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + + vehicle_status_s vehicle_status{}; + vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); + orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + + subscription_array.update(); + + // WHEN: we inject the vehicle_trajectory_waypoint in the interface + oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); + + // THEN: the setpoints shouldn't be injected + EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); + EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); + EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); + EXPECT_TRUE(vel_sp.isAllNan()); + EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); + EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); +} + +TEST_F(ObstacleAvoidanceTest, oa_desired) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + pos_sp = Vector3f(1.f, 1.2f, NAN); + vel_sp = Vector3f(NAN, NAN, 0.7f); + int type = 4; + Vector3f curr_wp(1.f, 1.2f, 5.0f); + float curr_yaw = 1.02f; + float curr_yawspeed = NAN; + Vector3f next_wp(11.f, 1.2f, 5.0f); + float next_yaw = 0.82f; + float next_yawspeed = NAN; + bool ext_yaw_active = false; + + // WHEN: we inject the setpoints and waypoints in the interface + oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, + type); + oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); + + // WHEN: we subscribe to the uORB message out of the interface + uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; + _sub_traj_wp_avoidance_desired.update(); + + // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 + EXPECT_FLOAT_EQ(pos_sp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); + EXPECT_FLOAT_EQ(pos_sp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); + EXPECT_FLOAT_EQ(vel_sp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); + EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); + + EXPECT_FLOAT_EQ(curr_wp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); + EXPECT_FLOAT_EQ(curr_wp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); + EXPECT_FLOAT_EQ(curr_wp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); + EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); + EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); + + EXPECT_FLOAT_EQ(next_wp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); + EXPECT_FLOAT_EQ(next_wp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); + EXPECT_FLOAT_EQ(next_wp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); + EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); + EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); + +} diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp index 98936eb57702..078564c01837 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,205 +37,54 @@ #include "StraightLine.hpp" #include -#include #include -#define VEL_ZERO_THRESHOLD 0.001f // Threshold to compare if the target velocity is zero -#define DECELERATION_MAX 8.0f // The vehicles maximum deceleration TODO check to create param - using namespace matrix; -StraightLine::StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos) : - ModuleParams(parent), - _deltatime(deltatime), _pos(pos) -{ - -} - void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint) { - // Check if target position has been reached - if (_desired_speed_at_target < VEL_ZERO_THRESHOLD && - (_pos - _target).length() < _param_nav_acc_rad.get()) { - // Vehicle has reached target. Lock position - position_setpoint = _target; - velocity_setpoint = Vector3f(0.0f, 0.0f, 0.0f); - + if (isEndReached()) { + // Vehicle has reached target, lock position + position_setpoint = _end; + velocity_setpoint.setNaN(); return; } - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); - // vector from origin to current position - Vector3f orig_to_pos = _pos - _origin; - // current position projected perpendicularly onto desired line - Vector3f closest_pt_on_line = _origin + u_orig_to_target * (orig_to_pos * u_orig_to_target); - // previous velocity in the direction of the line - float speed_sp_prev = math::max(velocity_setpoint * u_orig_to_target, 0.0f); - - // Calculate accelerating/decelerating distance depending on speed, speed at target and acceleration/deceleration - float acc_dec_distance = fabs((_desired_speed * _desired_speed) - (_desired_speed_at_target * - _desired_speed_at_target)) / 2.0f; - acc_dec_distance /= _desired_speed > _desired_speed_at_target ? _desired_deceleration : _desired_acceleration; - - float dist_to_target = (_target - _pos).length(); // distance to target - - // Either accelerate or decelerate - float speed_sp = dist_to_target > acc_dec_distance ? _desired_speed : _desired_speed_at_target; - float max_acc_dec = speed_sp > speed_sp_prev ? _desired_acceleration : -_desired_deceleration; - - float acc_track = 0.0f; - - if (_deltatime > FLT_EPSILON) { - acc_track = (speed_sp - speed_sp_prev) / _deltatime; - } - - if (fabs(acc_track) > fabs(max_acc_dec)) { - // accelerate/decelerate with desired acceleration/deceleration towards target - speed_sp = speed_sp_prev + max_acc_dec * _deltatime; - } - - // constrain the velocity - speed_sp = math::constrain(speed_sp, 0.0f, _desired_speed); - - // set the position and velocity setpoints - position_setpoint = closest_pt_on_line; - velocity_setpoint = u_orig_to_target * speed_sp; -} - -float StraightLine::getMaxAcc() -{ - // check if origin and target are different points - if ((_target - _origin).length() < FLT_EPSILON) { - return _param_mpc_acc_hor_max.get(); - } - - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); - - // calculate the maximal horizontal acceleration - float divider = Vector2f(u_orig_to_target).length(); - float max_acc_hor = _param_mpc_acc_hor_max.get(); - - if (divider > FLT_EPSILON) { - max_acc_hor /= divider; - - } else { - max_acc_hor *= 1000.0f; - } - - // calculate the maximal vertical acceleration - float max_acc_vert_original = u_orig_to_target(2) < 0 ? _param_mpc_acc_up_max.get() : _param_mpc_acc_down_max.get(); - float max_acc_vert = max_acc_vert_original; - - if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { - max_acc_vert /= fabs(u_orig_to_target(2)); - - } else { - max_acc_vert *= 1000.0f; - } - - return math::min(max_acc_hor, max_acc_vert); -} - -float StraightLine::getMaxVel() -{ - // check if origin and target are different points - if ((_target - _origin).length() < FLT_EPSILON) { - return _param_mpc_xy_vel_max.get(); - } - - // unit vector in the direction of the straight line - Vector3f u_orig_to_target = (_target - _origin).unit_or_zero(); + Vector3f start_to_end = _end - _start; + float distance_start_to_end = start_to_end.norm(); - // calculate the maximal horizontal velocity - float divider = Vector2f(u_orig_to_target).length(); - float max_vel_hor = _param_mpc_xy_vel_max.get(); + // capture progress as ratio between 0 and 1 of entire distance + Vector3f vehicle_to_end = _end - _position; + float distance_vehicle_to_end = vehicle_to_end.norm(); + float distance_from_start = Vector3f(_start - _position).norm(); + float progress = distance_from_start / (distance_from_start + distance_vehicle_to_end); - if (divider > FLT_EPSILON) { - max_vel_hor /= divider; + float distance_from_boundary = 0.f; - } else { - max_vel_hor *= 1000.0f; - } - - // calculate the maximal vertical velocity - float max_vel_vert_directional = u_orig_to_target(2) < 0 ? _param_mpc_z_vel_max_up.get() : - _param_mpc_z_vel_max_dn.get(); - float max_vel_vert = max_vel_vert_directional; - - if (fabs(u_orig_to_target(2)) > FLT_EPSILON) { - max_vel_vert /= fabs(u_orig_to_target(2)); + // calculate the distance to the closer boundary + if (progress < 0.5f) { + distance_from_boundary = (2 * progress) * distance_start_to_end; } else { - max_vel_vert *= 1000.0f; + distance_from_boundary = (2 * (1 - progress)) * distance_start_to_end; } - return math::min(max_vel_hor, max_vel_vert); -} + // ramp velocity based on the distance to the boundary + float velocity = 0.5f + (distance_from_boundary / 4.f); + velocity = math::min(velocity, _speed); + velocity_setpoint = vehicle_to_end.unit_or_zero() * velocity; -void StraightLine::setAllDefaults() -{ - _desired_speed = getMaxVel(); - _desired_speed_at_target = 0.0f; - _desired_acceleration = getMaxAcc(); - _desired_deceleration = DECELERATION_MAX; -} - -void StraightLine::setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target) -{ - if (PX4_ISFINITE(target(0)) && PX4_ISFINITE(target(1)) && PX4_ISFINITE(target(2)) && - PX4_ISFINITE(origin(0)) && PX4_ISFINITE(origin(1)) && PX4_ISFINITE(origin(2))) { - _target = target; - _origin = origin; - - // set all parameters to their default value (depends on the direction) - setAllDefaults(); + // check if we plan to go against the line direction which indicates we reached the goal + if (start_to_end * vehicle_to_end < 0) { + end_reached = true; } } -void StraightLine::setSpeed(const float &speed) +void StraightLine::setLineFromTo(const Vector3f &start, const Vector3f &end) { - float vel_max = getMaxVel(); - - if (speed > FLT_EPSILON && speed < vel_max) { - _desired_speed = speed; - - } else if (speed > vel_max) { - _desired_speed = vel_max; - } -} - -void StraightLine::setSpeedAtTarget(const float &speed_at_target) -{ - float vel_max = getMaxVel(); - - if (speed_at_target > FLT_EPSILON && speed_at_target < vel_max) { - _desired_speed_at_target = speed_at_target; - - } else if (speed_at_target > vel_max) { - _desired_speed_at_target = vel_max; - } -} - -void StraightLine::setAcceleration(const float &acc) -{ - float acc_max = getMaxVel(); - - if (acc > FLT_EPSILON && acc < acc_max) { - _desired_acceleration = acc; - - } else if (acc > acc_max) { - _desired_acceleration = acc_max; - } -} - -void StraightLine::setDeceleration(const float &dec) -{ - if (dec > FLT_EPSILON && dec < DECELERATION_MAX) { - _desired_deceleration = dec; - - } else if (dec > DECELERATION_MAX) { - _desired_deceleration = DECELERATION_MAX; + if (PX4_ISFINITE(start.norm_squared()) && PX4_ISFINITE(end.norm_squared())) { + _start = start; + _end = end; + end_reached = false; } } diff --git a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp index eaefdb4b42d8..5097999cde20 100644 --- a/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp +++ b/src/lib/FlightTasks/tasks/Utility/StraightLine.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,68 +41,39 @@ #pragma once -#include #include -class StraightLine : public ModuleParams +class StraightLine { public: - StraightLine(ModuleParams *parent, const float &deltatime, const matrix::Vector3f &pos); + StraightLine(const matrix::Vector3f &pos) : _position(pos) {}; ~StraightLine() = default; // setter functions - void setLineFromTo(const matrix::Vector3f &origin, const matrix::Vector3f &target); - void setSpeed(const float &speed); - void setSpeedAtTarget(const float &speed_at_target); - void setAcceleration(const float &acc); - void setDeceleration(const float &dec); + void setLineFromTo(const matrix::Vector3f &start, const matrix::Vector3f &end); + void setSpeed(const float &speed) { _speed = speed; }; /** - * Set all parameters to their default value depending on the direction of the line - */ - void setAllDefaults(); - - /** - * Get the maximum possible acceleration depending on the direction of the line - * Respects horizontal and vertical limits - */ - float getMaxAcc(); - - /** - * Get the maximum possible velocity depending on the direction of the line - * Respects horizontal and vertical limits + * Generate setpoints on a straight line according to parameters + * + * @param position_setpoint: reference to the 3D vector with the position setpoint to update + * @param velocity_setpoint: reference to the 3D vector with the velocity setpoint to update */ - float getMaxVel(); + void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint); /** - * Generate setpoints on a straight line according to parameters + * Check if the end was reached * - * @param position_setpoint: 3D vector with the previous position setpoint - * @param velocity_setpoint: 3D vector with the previous velocity setpoint + * @return false when on the way from start to end, true when end was reached */ - void generateSetpoints(matrix::Vector3f &position_setpoint, matrix::Vector3f &velocity_setpoint); + bool isEndReached() { return end_reached; } private: - const float &_deltatime; /**< delta time between last update (dependency injection) */ - const matrix::Vector3f &_pos; /**< vehicle position (dependency injection) */ - - float _desired_acceleration{0.0f}; /**< acceleration along the straight line */ - float _desired_deceleration{0.0f}; /**< deceleration along the straight line */ - float _desired_speed{0.0f}; /**< desired maximum velocity */ - float _desired_speed_at_target{0.0f}; /**< desired velocity at target point */ - - matrix::Vector3f _target{}; /**< End point of the straight line */ - matrix::Vector3f _origin{}; /**< Start point of the straight line */ + const matrix::Vector3f &_position; /**< vehicle position (dependency injection) */ - // parameters for default values - DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_acc_hor_max, /**< maximum horizontal acceleration */ - (ParamFloat) _param_mpc_acc_up_max, /**< maximum vertical acceleration upwards */ - (ParamFloat) _param_mpc_acc_down_max, /**< maximum vertical acceleration downwards*/ - (ParamFloat) _param_mpc_xy_vel_max, /**< maximum horizontal velocity */ - (ParamFloat) _param_mpc_z_vel_max_up, /**< maximum vertical velocity upwards */ - (ParamFloat) _param_mpc_z_vel_max_dn, /**< maximum vertical velocity downwards */ - (ParamFloat) _param_nav_acc_rad /**< acceptance radius if a waypoint is reached */ - ) + matrix::Vector3f _start; /**< Start point of the straight line */ + matrix::Vector3f _end; /**< End point of the straight line */ + float _speed = 1.f; /**< desired speed between accelerating and decelerating */ + bool end_reached = true; /**< Flag to lock further movement when end is reached */ }; diff --git a/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp b/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp new file mode 100644 index 000000000000..4bed8c4fa8b9 --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/TrajMath.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * 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 TrajMath.hpp + * + * collection of functions used in trajectory generators + */ + +#pragma once + +namespace trajmath +{ + +/* Compute the maximum possible speed on the track given the remaining distance, + * the maximum acceleration and the maximum jerk. + * We assume a constant acceleration profile with a delay of 2*accel/jerk + * (time to reach the desired acceleration from opposite max acceleration) + * Equation to solve: 0 = vel^2 - 2*accel*(x - vel*2*accel/jerk) + * + * @param jerk maximum jerk + * @param accel maximum acceleration + * @param braking_distance distance to the desired stopping point + * + * @return maximum speed + */ +template +const T computeMaxSpeedFromBrakingDistance(T jerk, T accel, T braking_distance) +{ + T b = (T) 4 * accel * accel / jerk; + T c = - (T) 2 * accel * braking_distance; + T max_speed = (T) 0.5 * (-b + sqrtf(b * b - (T) 4 * c)); + + return max_speed; +} + +/* Compute the maximum tangential speed in a circle defined by two line segments of length "d" + * forming a V shape, opened by an angle "alpha". The circle is tangent to the end of the + * two segments as shown below: + * \\ + * | \ d + * / \ + * __='___a\ + * d + * @param alpha angle between the two line segments + * @param accel maximum lateral acceleration + * @param d length of the two line segments + * + * @return maximum tangential speed + */ +template +const T computeMaxSpeedInWaypoint(T alpha, T accel, T d) +{ + T tan_alpha = tan(alpha / (T) 2); + T max_speed_in_turn = sqrtf(accel * d * tan_alpha); + + return max_speed_in_turn; +} +} /* namespace trajmath */ diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp new file mode 100644 index 000000000000..41e85b91e6df --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test code for the Velocity Smoothing library + * Run this test only using make tests TESTFILTER=VelocitySmoothing + */ + +#include +#include + +#include "VelocitySmoothing.hpp" + +using namespace matrix; + +class VelocitySmoothingTest : public ::testing::Test +{ +public: + void setConstraints(float j_max, float a_max, float v_max); + void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos); + void updateTrajectories(Vector3f velocity_setpoints, float dt); + + VelocitySmoothing _trajectories[3]; +}; + +void VelocitySmoothingTest::setConstraints(float j_max, float a_max, float v_max) +{ + for (int i = 0; i < 3; i++) { + _trajectories[i].setMaxJerk(j_max); + _trajectories[i].setMaxAccel(a_max); + _trajectories[i].setMaxVel(v_max); + } +} + +void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vector3f x0) +{ + for (int i = 0; i < 3; i++) { + _trajectories[i].setCurrentAcceleration(a0(i)); + _trajectories[i].setCurrentVelocity(v0(i)); + _trajectories[i].setCurrentPosition(x0(i)); + } +} + +void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float dt) +{ + for (int i = 0; i < 3; i++) { + _trajectories[i].updateDurations(dt, velocity_setpoints(i)); + } + + VelocitySmoothing::timeSynchronization(_trajectories, 2); + + float dummy; // We don't care about the immediate result + + for (int i = 0; i < 3; i++) { + _trajectories[i].integrate(dummy, dummy, dummy); + } +} + +TEST_F(VelocitySmoothingTest, testTimeSynchronization) +{ + // GIVEN: A set of constraints + const float j_max = 55.2f; + const float a_max = 6.f; + const float v_max = 6.f; + + setConstraints(j_max, a_max, v_max); + + // AND: A set of initial conditions + Vector3f a0(0.f, 0.f, 0.f); + Vector3f v0(0.f, 0.f, 0.f); + Vector3f x0(0.f, 0.f, 0.f); + + setInitialConditions(a0, v0, x0); + + // WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt + Vector3f velocity_setpoints(0.f, 1.f, 0.f); + float dt = 0.01f; + updateTrajectories(velocity_setpoints, dt); + + // THEN: The X and Y trajectories should have the same total time (= time sunchronized) + EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001); +} + +TEST_F(VelocitySmoothingTest, testConstantSetpoint) +{ + // GIVEN: A set of initial conditions (same constraints as before) + Vector3f a0(0.22f, 0.f, 0.22f); + Vector3f v0(2.47f, -5.59e-6f, 2.47f); + Vector3f x0(0.f, 0.f, 0.f); + + setInitialConditions(a0, v0, x0); + + // WHEN: We generate trajectories with constant setpoints and dt + Vector3f velocity_setpoints(0.f, 1.f, 0.f); + float dt = 0.01f; + + // Compute the number of steps required to reach desired value + // because of known numerical issues, the actual trajectory takes a + // bit more time than the predicted one, this is why we have to add 14 steps + // to the theoretical value. + // The updateTrajectories is fist called once to compute the total time + updateTrajectories(velocity_setpoints, dt); + float t123 = _trajectories[0].getTotalTime(); + int nb_steps = ceil(t123 / dt) + 14; + + for (int i = 0; i < nb_steps; i++) { + updateTrajectories(velocity_setpoints, dt); + } + + // THEN: All the trajectories should have reach their + // final state: desired velocity target and zero acceleration + for (int i = 0; i < 3; i++) { + EXPECT_LE(fabsf(_trajectories[i].getCurrentVelocity() - velocity_setpoints(i)), 0.01f); + EXPECT_LE(fabsf(_trajectories[i].getCurrentAcceleration()), 0.0001f); + } +} + +TEST_F(VelocitySmoothingTest, testZeroSetpoint) +{ + // GIVEN: A set of null initial conditions + Vector3f a0(0.f, 0.f, 0.f); + Vector3f v0(0.f, 0.f, 0.f); + Vector3f x0(0.f, 0.f, 0.f); + + setInitialConditions(a0, v0, x0); + + // AND: Null setpoints + Vector3f velocity_setpoints(0.f, 0.f, 0.f); + float dt = 0.01f; + + // WHEN: We run a few times the algorithm + for (int i = 0; i < 60; i++) { + updateTrajectories(velocity_setpoints, dt); + } + + // THEN: All the trajectories should still be null + for (int i = 0; i < 3; i++) { + EXPECT_EQ(_trajectories[i].getCurrentJerk(), 0.f); + EXPECT_EQ(_trajectories[i].getCurrentAcceleration(), 0.f); + EXPECT_EQ(_trajectories[i].getCurrentVelocity(), 0.f); + EXPECT_EQ(_trajectories[i].getCurrentPosition(), 0.f); + } +} diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index fbf06df6e7e3..7fa0040b5674 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -55,8 +55,8 @@ * @param differential_pressure total_ pressure - static pressure * @return indicated airspeed in m/s */ -float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, - float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) +float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, + float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) { // air density in kg/m3 @@ -184,16 +184,15 @@ float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, /** - * Calculate indicated airspeed. + * Calculate indicated airspeed (IAS). * * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. + * lacks the air density and instrument error compensation. * * @param differential_pressure total_ pressure - static pressure - * @return indicated airspeed in m/s + * @return IAS in m/s */ -float calc_indicated_airspeed(float differential_pressure) +float calc_IAS(float differential_pressure) { @@ -207,32 +206,47 @@ float calc_indicated_airspeed(float differential_pressure) } /** - * Calculate true airspeed from indicated airspeed. + * Calculate true airspeed (TAS) from equivalent airspeed (EAS). * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed_indicated current indicated airspeed + * @param speed_equivalent current equivalent airspeed * @param pressure_ambient pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s + * @return TAS in m/s */ -float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) +float calc_TAS_from_EAS(float speed_equivalent, float pressure_ambient, float temperature_celsius) { - return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, - temperature_celsius)); + return speed_equivalent * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + temperature_celsius)); } /** - * Directly calculate true airspeed + * Calculate equivalent airspeed (EAS) from indicated airspeed (IAS). + * Note that we neglect the conversion from CAS (calibrated airspeed) to EAS. * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * @param speed_indicated current indicated airspeed + * @param scale scale from IAS to CAS (accounting for instrument and pitot position erros) + * @return EAS in m/s + */ +float calc_EAS_from_IAS(float speed_indicated, float scale) +{ + return speed_indicated * scale; +} + +/** + * Directly calculate true airspeed (TAS) + * + * Here we assume to have no instrument or pitot position error (IAS = CAS), + * and neglect the CAS to EAS conversion (CAS = EAS). + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind. * * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) +float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); @@ -254,3 +268,19 @@ float get_air_density(float static_pressure, float temperature_celsius) { return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } + +/** + * Calculate equivalent airspeed (EAS) from true airspeed (TAS). + * It is the inverse function to calc_TAS_from_EAS() + * + * + * @param speed_true current true airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return EAS in m/s + */ +float calc_EAS_from_TAS(float speed_true, float pressure_ambient, float temperature_celsius) +{ + return speed_true * sqrtf(get_air_density(pressure_ambient, + temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); +} diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index b3eebc0eab00..43aa305efc4f 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -58,7 +58,7 @@ enum AIRSPEED_COMPENSATION_MODEL { }; /** - * Calculate indicated airspeed. + * Calculate indicated airspeed (IAS). * * Note that the indicated airspeed is not the true airspeed because it * lacks the air density compensation. Use the calc_true_airspeed functions to get @@ -68,12 +68,12 @@ enum AIRSPEED_COMPENSATION_MODEL { * @param static_pressure pressure at the side of the tube/airplane * @return indicated airspeed in m/s */ -__EXPORT float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, - enum AIRSPEED_SENSOR_MODEL smodel, - float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius); +__EXPORT float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, + enum AIRSPEED_SENSOR_MODEL smodel, + float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius); /** - * Calculate indicated airspeed. + * Calculate indicated airspeed (IAS). * * Note that the indicated airspeed is not the true airspeed because it * lacks the air density compensation. Use the calc_true_airspeed functions to get @@ -83,32 +83,45 @@ __EXPORT float calc_indicated_airspeed_corrected(enum AIRSPEED_COMPENSATION_MODE * @param static_pressure pressure at the side of the tube/airplane * @return indicated airspeed in m/s */ -__EXPORT float calc_indicated_airspeed(float differential_pressure); +__EXPORT float calc_IAS(float differential_pressure); /** - * Calculate true airspeed from indicated airspeed. + * Calculate true airspeed (TAS) from equivalent airspeed (EAS). * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed_indicated current indicated airspeed + * @param speed_equivalent current equivalent airspeed * @param pressure_ambient pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius - * @return true airspeed in m/s + * @return TAS in m/s */ -__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, - float temperature_celsius); +__EXPORT float calc_TAS_from_EAS(float speed_indicated, float pressure_ambient, + float temperature_celsius); /** - * Directly calculate true airspeed + * Calculate equivalent airspeed (EAS) from indicated airspeed (IAS). + * Note that we neglect the conversion from CAS (calibrated airspeed) to EAS. * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * @param speed_indicated current indicated airspeed + * @param scale scale from IAS to CAS (accounting for instrument and pitot position erros) + * @return EAS in m/s + */ +__EXPORT float calc_EAS_from_IAS(float speed_indicated, float scale); + + +/** + * Directly calculate true airspeed (TAS) + * + * Here we assume to have no instrument or pitot position error (IAS = CAS), + * and neglect the CAS to EAS conversion (CAS = EAS). + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind. * * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); +__EXPORT float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius); /** * Calculates air density. @@ -118,6 +131,19 @@ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, f */ __EXPORT float get_air_density(float static_pressure, float temperature_celsius); +/** + * Calculate equivalent airspeed (EAS) from true airspeed (TAS). + * It is the inverse function to calc_TAS_from_EAS() + * + * + * @param speed_true current true airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return EAS in m/s + */ +__EXPORT float calc_EAS_from_TAS(float speed_true, float pressure_ambient, + float temperature_celsius); + __END_DECLS #endif diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index e550c56c5b39..7d9a2e506fed 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -97,6 +97,8 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre battery_status->system_source = selected_source; battery_status->priority = priority; } + + battery_status->temperature = NAN; } void diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index 78f6282dec56..6955cdffc13d 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -395,4 +395,27 @@ CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) return -EINVAL; } +int CDev::unregister_driver_and_memory() +{ + int retval = PX4_OK; + + if (_registered) { + unregister_driver(_devname); + _registered = false; + + } else { + retval = -ENODEV; + } + + if (_devname != nullptr) { + free((void *)_devname); + _devname = nullptr; + + } else { + retval = -ENODEV; + } + + return retval; +} + } // namespace cdev diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index af036e8baadd..f51ed45a3d95 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -264,6 +264,17 @@ class __EXPORT CDev px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ + + /** + * First, unregisters the driver. Next, free the memory for the devname, + * in case it was expected to have ownership. Sets devname to nullptr. + * + * This is only needed if the ownership of the devname was passed to the CDev, otherwise ~CDev handles it. + * + * @return PX4_OK on success, -ENODEV if the devname is already nullptr + */ + int unregister_driver_and_memory(); + private: const char *_devname{nullptr}; /**< device node name */ diff --git a/src/lib/cdev/test/cdevtest_main.cpp b/src/lib/cdev/test/cdevtest_main.cpp index cf4c05a42d39..e642e80b9e9e 100644 --- a/src/lib/cdev/test/cdevtest_main.cpp +++ b/src/lib/cdev/test/cdevtest_main.cpp @@ -39,8 +39,8 @@ */ #include "cdevtest_example.h" -#include #include +#include #include int PX4_MAIN(int argc, char **argv) diff --git a/src/lib/controllib/BlockLowPass.hpp b/src/lib/controllib/BlockLowPass.hpp index 73f2a51dc545..9302977408eb 100644 --- a/src/lib/controllib/BlockLowPass.hpp +++ b/src/lib/controllib/BlockLowPass.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -65,7 +66,7 @@ class __EXPORT BlockLowPass : public Block // methods BlockLowPass(SuperBlock *parent, const char *name) : Block(parent, name), - _state(0.0f / 0.0f /* initialize to invalid val, force into is_finite() check on first call */), + _state(NAN /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, "") // only one parameter, no need to name {} virtual ~BlockLowPass() {} diff --git a/src/lib/controllib/BlockStats.hpp b/src/lib/controllib/BlockStats.hpp index 9f26ad6004ee..22c1359f6218 100644 --- a/src/lib/controllib/BlockStats.hpp +++ b/src/lib/controllib/BlockStats.hpp @@ -89,7 +89,7 @@ class __EXPORT BlockStats: public Block } matrix::Vector getStdDev() { - return getVar().pow(0.5f); + return getVar().sqrt(); } private: // attributes diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 0b508c516414..d2d6730e26fc 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -277,5 +277,13 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) x = tmp; return; } + + case ROTATION_ROLL_90_YAW_270: { + tmp = x; + x = -z; + z = y; + y = -tmp; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index d2281774771a..ce62e8d66736 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -84,6 +84,7 @@ enum Rotation { ROTATION_PITCH_9_YAW_180 = 32, ROTATION_PITCH_45 = 33, ROTATION_PITCH_315 = 34, + ROTATION_ROLL_90_YAW_270 = 35, ROTATION_MAX }; @@ -129,6 +130,7 @@ const rot_lookup_t rot_lookup[] = { { 0, 9, 180 }, { 0, 45, 0 }, { 0, 315, 0 }, + { 90, 0, 270 }, }; /** diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 81e7612b73dd..3995d705bf38 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -41,4 +41,3 @@ add_subdirectory(linux_gpio) add_subdirectory(magnetometer) add_subdirectory(rangefinder) add_subdirectory(smbus) -add_subdirectory(tone_alarm) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 1e0363f18ee2..8b74a8122e11 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -50,10 +50,6 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro // set software low pass filter for controllers updateParams(); configure_filter(_param_imu_accel_cutoff.get()); - - // force initial publish to allocate uORB buffer - // TODO: can be removed once all drivers are in threads - _sensor_accel_pub.update(); } PX4Accelerometer::~PX4Accelerometer() @@ -63,7 +59,8 @@ PX4Accelerometer::~PX4Accelerometer() } } -int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +int +PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case ACCELIOCSSCALE: { @@ -85,7 +82,8 @@ int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) } } -void PX4Accelerometer::set_device_type(uint8_t devtype) +void +PX4Accelerometer::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; @@ -98,13 +96,15 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) _sensor_accel_pub.get().device_id = device_id.devid; } -void PX4Accelerometer::set_sample_rate(unsigned rate) +void +PX4Accelerometer::set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); } -void PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) +void +PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) { sensor_accel_s &report = _sensor_accel_pub.get(); report.timestamp = timestamp; @@ -145,7 +145,8 @@ void PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) } } -void PX4Accelerometer::print_status() +void +PX4Accelerometer::print_status() { PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); PX4_INFO("sample rate: %d Hz", _sample_rate); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 604c8291e5f3..55af0320c614 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include class PX4Accelerometer : public cdev::CDev, public ModuleParams @@ -68,7 +68,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_accel_pub; + uORB::PublicationMultiData _sensor_accel_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, false}; diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h index f3742675484a..aff712c051df 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/lib/drivers/airspeed/airspeed.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include /* Default I2C bus */ static constexpr uint8_t PX4_I2C_BUS_DEFAULT = PX4_I2C_BUS_EXPANSION; diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index 599d529382b3..7307ce3448cd 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -43,10 +43,6 @@ PX4Barometer::PX4Barometer(uint32_t device_id, uint8_t priority) : _class_device_instance = register_class_devname(BARO_BASE_DEVICE_PATH); _sensor_baro_pub.get().device_id = device_id; - - // force initial publish to allocate uORB buffer - // TODO: can be removed once all drivers are in threads - _sensor_baro_pub.update(); } PX4Barometer::~PX4Barometer() @@ -56,7 +52,8 @@ PX4Barometer::~PX4Barometer() } } -void PX4Barometer::set_device_type(uint8_t devtype) +void +PX4Barometer::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; @@ -69,7 +66,8 @@ void PX4Barometer::set_device_type(uint8_t devtype) _sensor_baro_pub.get().device_id = device_id.devid; } -void PX4Barometer::update(hrt_abstime timestamp, float pressure) +void +PX4Barometer::update(hrt_abstime timestamp, float pressure) { sensor_baro_s &report = _sensor_baro_pub.get(); @@ -81,7 +79,8 @@ void PX4Barometer::update(hrt_abstime timestamp, float pressure) _sensor_baro_pub.update(); } -void PX4Barometer::print_status() +void +PX4Barometer::print_status() { PX4_INFO(BARO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index a72e4038d17e..ca48928119a3 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Barometer : public cdev::CDev @@ -59,7 +59,7 @@ class PX4Barometer : public cdev::CDev private: - uORB::PublicationData _sensor_baro_pub; + uORB::PublicationMultiData _sensor_baro_pub; int _class_device_instance{-1}; diff --git a/src/lib/drivers/device/nuttx/SPI.cpp b/src/lib/drivers/device/nuttx/SPI.cpp index 2036716631a2..9b121dfb281c 100644 --- a/src/lib/drivers/device/nuttx/SPI.cpp +++ b/src/lib/drivers/device/nuttx/SPI.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -57,18 +57,8 @@ namespace device { -SPI::SPI(const char *name, - const char *devname, - int bus, - uint32_t device, - enum spi_mode_e mode, - uint32_t frequency) : - // base class +SPI::SPI(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency) : CDev(name, devname), - // public - // protected - locking_mode(LOCK_PREEMPTION), - // private _device(device), _mode(mode), _frequency(frequency), @@ -90,15 +80,12 @@ SPI::~SPI() int SPI::init() { - int ret = OK; - /* attach to the spi bus */ if (_dev == nullptr) { int bus = get_device_bus(); if (!board_has_bus(BOARD_SPI_BUS, bus)) { - ret = -ENOENT; - goto out; + return -ENOENT; } _dev = px4_spibus_initialize(bus); @@ -106,19 +93,18 @@ SPI::init() if (_dev == nullptr) { DEVICE_DEBUG("failed to init SPI"); - ret = -ENOENT; - goto out; + return -ENOENT; } /* deselect device to ensure high to low transition of pin select */ SPI_SELECT(_dev, _device, false); /* call the probe function to check whether the device is present */ - ret = probe(); + int ret = probe(); if (ret != OK) { DEVICE_DEBUG("probe failed"); - goto out; + return ret; } /* do base class init, which will create the device node, etc. */ @@ -126,14 +112,13 @@ SPI::init() if (ret != OK) { DEVICE_DEBUG("cdev init failed"); - goto out; + return ret; } /* tell the workd where we are */ DEVICE_LOG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000); -out: - return ret; + return PX4_OK; } int @@ -145,7 +130,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return -EINVAL; } - LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; + LockMode mode = up_interrupt_context() ? LOCK_NONE : _locking_mode; /* lock the bus as required */ switch (mode) { @@ -185,7 +170,7 @@ SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - return OK; + return PX4_OK; } int @@ -197,7 +182,7 @@ SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) return -EINVAL; } - LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; + LockMode mode = up_interrupt_context() ? LOCK_NONE : _locking_mode; /* lock the bus as required */ switch (mode) { @@ -228,7 +213,7 @@ SPI::_transferhword(uint16_t *send, uint16_t *recv, unsigned len) { SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); - SPI_SETBITS(_dev, 16); /* 16 bit transfer */ + SPI_SETBITS(_dev, 16); /* 16 bit transfer */ SPI_SELECT(_dev, _device, true); /* do the transfer */ @@ -237,7 +222,7 @@ SPI::_transferhword(uint16_t *send, uint16_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - return OK; + return PX4_OK; } } // namespace device diff --git a/src/lib/drivers/device/nuttx/SPI.hpp b/src/lib/drivers/device/nuttx/SPI.hpp index 24e126375110..ac83eb83ab7f 100644 --- a/src/lib/drivers/device/nuttx/SPI.hpp +++ b/src/lib/drivers/device/nuttx/SPI.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file spi.h + * @file SPI.hpp * * Base class for devices connected via SPI. */ @@ -61,12 +61,7 @@ class __EXPORT SPI : public CDev * @param mode SPI clock/data mode * @param frequency SPI clock frequency */ - SPI(const char *name, - const char *devname, - int bus, - uint32_t device, - enum spi_mode_e mode, - uint32_t frequency); + SPI(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency); virtual ~SPI(); /** @@ -137,6 +132,7 @@ class __EXPORT SPI : public CDev * @param frequency Frequency to set (Hz) */ void set_frequency(uint32_t frequency) { _frequency = frequency; } + uint32_t get_frequency() { return _frequency; } /** * Set the SPI bus locking mode @@ -146,16 +142,16 @@ class __EXPORT SPI : public CDev * * @param mode Locking mode */ - void set_lockmode(enum LockMode mode) { locking_mode = mode; } - - LockMode locking_mode; /**< selected locking mode */ + void set_lockmode(enum LockMode mode) { _locking_mode = mode; } private: - uint32_t _device; + uint32_t _device; enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + LockMode _locking_mode{LOCK_THREADS}; /**< selected locking mode */ + /* this class does not allow copying */ SPI(const SPI &); SPI operator=(const SPI &); diff --git a/src/lib/drivers/device/posix/SPI.cpp b/src/lib/drivers/device/posix/SPI.cpp index 9c021d03e6a1..e9f1a2f06fc5 100644 --- a/src/lib/drivers/device/posix/SPI.cpp +++ b/src/lib/drivers/device/posix/SPI.cpp @@ -80,8 +80,6 @@ SPI::~SPI() int SPI::init() { - int ret = OK; - // Open the actual SPI device char dev_path[16]; snprintf(dev_path, sizeof(dev_path), "/dev/spidev%i.%i", get_device_bus(), PX4_SPI_DEV_ID(_device)); @@ -94,7 +92,7 @@ SPI::init() } /* call the probe function to check whether the device is present */ - ret = probe(); + int ret = probe(); if (ret != OK) { DEVICE_DEBUG("probe failed"); diff --git a/src/lib/drivers/device/posix/SPI.hpp b/src/lib/drivers/device/posix/SPI.hpp index 6ce5825fc4c0..9f0a1755c3bb 100644 --- a/src/lib/drivers/device/posix/SPI.hpp +++ b/src/lib/drivers/device/posix/SPI.hpp @@ -79,15 +79,6 @@ class __EXPORT SPI : public CDev SPI(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency); virtual ~SPI(); - /** - * Locking modes supported by the driver. - */ - enum LockMode { - LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ - LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ - LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ - }; - virtual int init(); /** @@ -147,17 +138,7 @@ class __EXPORT SPI : public CDev * @param frequency Frequency to set (Hz) */ void set_frequency(uint32_t frequency) { _frequency = frequency; } - uint32_t get_frequency() { return _frequency; } - - /** - * Set the SPI bus locking mode - * - * This set the SPI locking mode. For devices competing with NuttX SPI - * drivers on a bus the right lock mode is LOCK_THREADS. - * - * @param mode Locking mode - */ - void set_lockmode(enum LockMode mode) {} + uint32_t get_frequency() { return _frequency; } private: int _fd{-1}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index a4622e69fd09..07a11d07c95f 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -40,20 +40,18 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r CDev(nullptr), ModuleParams(nullptr), _sensor_gyro_pub{ORB_ID(sensor_gyro), priority}, + _sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority}, _rotation{rotation} { _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); _sensor_gyro_pub.get().device_id = device_id; _sensor_gyro_pub.get().scaling = 1.0f; + _sensor_gyro_control_pub.get().device_id = device_id; // set software low pass filter for controllers updateParams(); configure_filter(_param_imu_gyro_cutoff.get()); - - // force initial publish to allocate uORB buffer - // TODO: can be removed once all drivers are in threads - _sensor_gyro_pub.update(); } PX4Gyroscope::~PX4Gyroscope() @@ -63,7 +61,8 @@ PX4Gyroscope::~PX4Gyroscope() } } -int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +int +PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case GYROIOCSSCALE: { @@ -85,7 +84,8 @@ int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) } } -void PX4Gyroscope::set_device_type(uint8_t devtype) +void +PX4Gyroscope::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; @@ -96,15 +96,18 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) // copy back to report _sensor_gyro_pub.get().device_id = device_id.devid; + _sensor_gyro_control_pub.get().device_id = device_id.devid; } -void PX4Gyroscope::set_sample_rate(unsigned rate) +void +PX4Gyroscope::set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); } -void PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) +void +PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) { sensor_gyro_s &report = _sensor_gyro_pub.get(); report.timestamp = timestamp; @@ -120,6 +123,27 @@ void PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) // Filtered values const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)}; + + // publish control data (filtered gyro) immediately + bool publish_control = true; + sensor_gyro_control_s &control = _sensor_gyro_control_pub.get(); + + if (_param_imu_gyro_rate_max.get() > 0) { + const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + + if (hrt_elapsed_time(&control.timestamp_sample) < interval) { + publish_control = false; + } + } + + if (publish_control) { + control.timestamp_sample = timestamp; + val_filtered.copyTo(control.xyz); + control.timestamp = hrt_absolute_time(); + _sensor_gyro_control_pub.update(); // publish + } + + // Integrated values matrix::Vector3f integrated_value; uint32_t integral_dt = 0; @@ -141,11 +165,12 @@ void PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) report.z_integral = integrated_value(2); poll_notify(POLLIN); - _sensor_gyro_pub.update(); + _sensor_gyro_pub.update(); // publish } } -void PX4Gyroscope::print_status() +void +PX4Gyroscope::print_status() { PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); PX4_INFO("sample rate: %d Hz", _sample_rate); @@ -157,4 +182,5 @@ void PX4Gyroscope::print_status() (double)_calibration_offset(2)); print_message(_sensor_gyro_pub.get()); + print_message(_sensor_gyro_control_pub.get()); } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 6868038b46e9..7ee7bffbde0b 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -41,8 +41,9 @@ #include #include #include -#include +#include #include +#include class PX4Gyroscope : public cdev::CDev, public ModuleParams { @@ -68,7 +69,8 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_gyro_pub; + uORB::PublicationMultiData _sensor_gyro_pub; + uORB::PublicationMultiData _sensor_gyro_control_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; @@ -83,7 +85,8 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams unsigned _sample_rate{1000}; DEFINE_PARAMETERS( - (ParamFloat) _param_imu_gyro_cutoff + (ParamFloat) _param_imu_gyro_cutoff, + (ParamInt) _param_imu_gyro_rate_max ) }; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index a66caeccef26..8c697c4f4421 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -45,10 +45,6 @@ PX4Magnetometer::PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rota _sensor_mag_pub.get().device_id = device_id; _sensor_mag_pub.get().scaling = 1.0f; - - // force initial publish to allocate uORB buffer - // TODO: can be removed once all drivers are in threads - _sensor_mag_pub.update(); } PX4Magnetometer::~PX4Magnetometer() @@ -58,7 +54,8 @@ PX4Magnetometer::~PX4Magnetometer() } } -int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) +int +PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { case MAGIOCSSCALE: { @@ -94,7 +91,8 @@ int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) } } -void PX4Magnetometer::set_device_type(uint8_t devtype) +void +PX4Magnetometer::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; @@ -107,7 +105,8 @@ void PX4Magnetometer::set_device_type(uint8_t devtype) _sensor_mag_pub.get().device_id = device_id.devid; } -void PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) +void +PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) { sensor_mag_s &report = _sensor_mag_pub.get(); report.timestamp = timestamp; @@ -136,7 +135,8 @@ void PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_ _sensor_mag_pub.update(); } -void PX4Magnetometer::print_status() +void +PX4Magnetometer::print_status() { PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance); diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index 545a10ace4f4..e909602dd881 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Magnetometer : public cdev::CDev @@ -63,7 +63,7 @@ class PX4Magnetometer : public cdev::CDev private: - uORB::PublicationData _sensor_mag_pub; + uORB::PublicationMultiData _sensor_mag_pub; const enum Rotation _rotation; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 5574a72315ee..38c86c0a4857 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp @@ -35,17 +35,14 @@ #include -PX4Rangefinder::PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation) : +PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t priority, const uint8_t device_orientation) : CDev(nullptr), - _distance_sensor_pub{ORB_ID(distance_sensor), priority}, - _rotation{rotation} + _distance_sensor_pub{ORB_ID(distance_sensor), priority} { _class_device_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - // TODO: range finders should have device ids - //_distance_sensor_pub.get().device_id = device_id; - - _distance_sensor_pub.get().orientation = rotation; + set_device_id(device_id); + set_orientation(device_orientation); } PX4Rangefinder::~PX4Rangefinder() @@ -56,7 +53,7 @@ PX4Rangefinder::~PX4Rangefinder() } void -PX4Rangefinder::set_device_type(uint8_t devtype) +PX4Rangefinder::set_device_type(uint8_t device_type) { // TODO: range finders should have device ids @@ -72,13 +69,19 @@ PX4Rangefinder::set_device_type(uint8_t devtype) } void -PX4Rangefinder::update(hrt_abstime timestamp, float distance, int8_t quality) +PX4Rangefinder::set_orientation(const uint8_t device_orientation) +{ + _distance_sensor_pub.get().orientation = device_orientation; +} + +void +PX4Rangefinder::update(const hrt_abstime timestamp, const float distance, const int8_t quality) { distance_sensor_s &report = _distance_sensor_pub.get(); - report.timestamp = timestamp; report.current_distance = distance; - report.signal_quality = quality; + report.signal_quality = quality; + report.timestamp = timestamp; _distance_sensor_pub.update(); } diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index 211f605072d8..bafb34c8d089 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -38,36 +38,40 @@ #include #include #include -#include +#include #include class PX4Rangefinder : public cdev::CDev { public: - PX4Rangefinder(uint32_t device_id, uint8_t priority, uint8_t rotation); + PX4Rangefinder(const uint32_t device_id, + const uint8_t priority, + const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); ~PX4Rangefinder() override; - void set_device_type(uint8_t devtype); + void print_status(); + + void set_device_type(uint8_t device_type); //void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; } - void set_min_distance(float distance) { _distance_sensor_pub.get().min_distance = distance; } - void set_max_distance(float distance) { _distance_sensor_pub.get().max_distance = distance; } + void set_device_id(const uint8_t device_id) { _distance_sensor_pub.get().id = device_id; }; - void set_hfov(float fov) { _distance_sensor_pub.get().h_fov = fov; } - void set_vfov(float fov) { _distance_sensor_pub.get().v_fov = fov; } - void set_fov(float fov) { set_hfov(fov); set_vfov(fov); } + void set_fov(const float fov) { set_hfov(fov); set_vfov(fov); } + void set_hfov(const float fov) { _distance_sensor_pub.get().h_fov = fov; } + void set_vfov(const float fov) { _distance_sensor_pub.get().v_fov = fov; } - void update(hrt_abstime timestamp, float distance, int8_t quality = -1); + void set_max_distance(const float distance) { _distance_sensor_pub.get().max_distance = distance; } + void set_min_distance(const float distance) { _distance_sensor_pub.get().min_distance = distance; } - void print_status(); + void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); -private: + void update(const hrt_abstime timestamp, const float distance, const int8_t quality = -1); - uORB::PublicationData _distance_sensor_pub; +private: - const uint8_t _rotation; + uORB::PublicationMultiData _distance_sensor_pub; - int _class_device_instance{-1}; + int _class_device_instance{-1}; }; diff --git a/src/lib/drivers/tone_alarm/ToneAlarmInterface.h b/src/lib/drivers/tone_alarm/ToneAlarmInterface.h deleted file mode 100644 index 9d2b35d3bd72..000000000000 --- a/src/lib/drivers/tone_alarm/ToneAlarmInterface.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ToneAlarmInterface.cpp - */ - -namespace ToneAlarmInterface -{ -/** - * @brief Activates/configures the hardware registers. - */ -void init(); - -/** - * @brief Starts playing the note. - */ -void start_note(unsigned frequency); - -/** - * @brief Stops playing the current note and makes the player 'safe'. - */ -void stop_note(); -}; // ToneAlarmInterface diff --git a/src/lib/ecl b/src/lib/ecl index d38164fc8e68..3b32ee41660a 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit d38164fc8e68b740405669127138e550c3c7375c +Subproject commit 3b32ee41660afd4785c374355e0fdefdae83e9b9 diff --git a/src/lib/hysteresis/CMakeLists.txt b/src/lib/hysteresis/CMakeLists.txt index 1943cdc1aadc..bd71aa5ef674 100644 --- a/src/lib/hysteresis/CMakeLists.txt +++ b/src/lib/hysteresis/CMakeLists.txt @@ -33,4 +33,4 @@ px4_add_library(hysteresis hysteresis.cpp) -px4_add_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) +px4_add_unit_gtest(SRC HysteresisTest.cpp LINKLIBS hysteresis) diff --git a/src/lib/led/CMakeLists.txt b/src/lib/led/CMakeLists.txt index 1306dbc12e94..82e33bc292ad 100644 --- a/src/lib/led/CMakeLists.txt +++ b/src/lib/led/CMakeLists.txt @@ -32,3 +32,6 @@ ############################################################################ px4_add_library(led led.cpp) +target_compile_options(led + PRIVATE -Wno-implicit-fallthrough # TODO: fix and remove +) diff --git a/src/lib/led/led.cpp b/src/lib/led/led.cpp index 120a8eb4f916..e4202e6078f3 100644 --- a/src/lib/led/led.cpp +++ b/src/lib/led/led.cpp @@ -38,24 +38,13 @@ #include "led.h" -int LedController::init(int led_control_sub) -{ - _led_control_sub = led_control_sub; - _last_update_call = hrt_absolute_time(); - return 0; -} - int LedController::update(LedControlData &control_data) { - bool updated = false; - - orb_check(_led_control_sub, &updated); - - while (updated || _force_update) { + while (_led_control_sub.updated() || _force_update) { // handle new state led_control_s led_control; - if (orb_copy(ORB_ID(led_control), _led_control_sub, &led_control) == 0) { + if (_led_control_sub.copy(&led_control)) { // don't apply the new state just yet to avoid interrupting an ongoing blinking state for (int i = 0; i < BOARD_MAX_LEDS; ++i) { @@ -80,14 +69,18 @@ int LedController::update(LedControlData &control_data) } _force_update = false; - - orb_check(_led_control_sub, &updated); } bool had_changes = false; // did one of the outputs change? // handle state updates hrt_abstime now = hrt_absolute_time(); + + if (_last_update_call == 0) { + _last_update_call = now; + return 0; + } + uint16_t blink_delta_t = (uint16_t)((now - _last_update_call) / 100); // Note: this is in 0.1ms constexpr uint16_t breathe_duration = BREATHE_INTERVAL * BREATHE_STEPS / 100; diff --git a/src/lib/led/led.h b/src/lib/led/led.h index e6cc84482b63..93fa9166e42f 100644 --- a/src/lib/led/led.h +++ b/src/lib/led/led.h @@ -44,6 +44,8 @@ #include #include +#include +#include struct LedControlDataSingle { uint8_t color; ///< one of led_control_s::COLOR_* @@ -64,18 +66,6 @@ class LedController LedController() = default; ~LedController() = default; - /** - * initialize. Call this once before using the object - * @param led_control_sub uorb subscription for led_control - * @return 0 on success, <0 on error otherwise - */ - int init(int led_control_sub); - - /** - * check if already initialized - */ - bool is_init() const { return _led_control_sub >= 0; } - /** * get maxium time between two consecutive calls to update() in us. */ @@ -92,18 +82,16 @@ class LedController */ int update(LedControlData &control_data); - static const int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */ - static const int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */ + static constexpr int BREATHE_INTERVAL = 25 * 1000; /**< single step when in breathe mode */ + static constexpr int BREATHE_STEPS = 64; /**< number of steps in breathe mode for a full on-off cycle */ - static const int BLINK_FAST_DURATION = 100 * 1000; /**< duration of half a blinking cycle + static constexpr int BLINK_FAST_DURATION = 100 * 1000; /**< duration of half a blinking cycle (on-to-off and off-to-on) in us */ - static const int BLINK_NORMAL_DURATION = 500 * 1000; /**< duration of half a blinking cycle + static constexpr int BLINK_NORMAL_DURATION = 500 * 1000; /**< duration of half a blinking cycle (on-to-off and off-to-on) in us */ - static const int BLINK_SLOW_DURATION = 2000 * 1000; /**< duration of half a blinking cycle + static constexpr int BLINK_SLOW_DURATION = 2000 * 1000; /**< duration of half a blinking cycle (on-to-off and off-to-on) in us */ - int led_control_subscription() const { return _led_control_sub; } - private: /** set control_data based on current Led states */ @@ -186,8 +174,8 @@ class LedController PerLedData _states[BOARD_MAX_LEDS]; ///< keep current LED states - int _led_control_sub = -1; ///< uorb subscription - hrt_abstime _last_update_call; - bool _force_update = true; ///< force an orb_copy in the beginning - bool _breathe_enabled = false; ///< true if at least one of the led's is currently in breathe mode + uORB::Subscription _led_control_sub{ORB_ID(led_control)}; ///< uorb subscription + hrt_abstime _last_update_call{0}; + bool _force_update{true}; ///< force an orb_copy in the beginning + bool _breathe_enabled{false}; ///< true if at least one of the led's is currently in breathe mode }; diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 14dcd5f1699e..0cc394547a8a 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -48,7 +48,7 @@ namespace math template int sign(T val) { - return (T(0) < val) - (val < T(0)); + return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON)); } // Type-safe signum function with zero treated as positive diff --git a/src/lib/matrix b/src/lib/matrix index 56b069956da1..15865b741ccc 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 56b069956da141da244926ed7000e89b2ba6c731 +Subproject commit 15865b741ccc49322a8ffd445d9814eba9f94710 diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h index 5aa632421e27..5bf02129cb12 100644 --- a/src/lib/mixer/mixer.h +++ b/src/lib/mixer/mixer.h @@ -254,6 +254,8 @@ class Mixer */ virtual void set_airmode(Airmode airmode) {}; + virtual unsigned get_multirotor_count() {return 0;} + protected: /** client-supplied callback used when fetching control values */ ControlCallback _control_cb; @@ -450,6 +452,8 @@ class MixerGroup : public Mixer void set_airmode(Airmode airmode) override; + unsigned get_multirotor_count() override; + private: Mixer *_first; /**< linked list of mixers */ @@ -715,6 +719,8 @@ class MultirotorMixer : public Mixer void set_airmode(Airmode airmode) override; + unsigned get_multirotor_count() override {return _rotor_count;} + union saturation_status { struct { uint16_t valid : 1; // 0 - true when the saturation status is used diff --git a/src/lib/mixer/mixer_group.cpp b/src/lib/mixer/mixer_group.cpp index 1757d640ee67..92d8756cc800 100644 --- a/src/lib/mixer/mixer_group.cpp +++ b/src/lib/mixer/mixer_group.cpp @@ -198,6 +198,24 @@ MixerGroup::set_airmode(Airmode airmode) } } +unsigned +MixerGroup::get_multirotor_count() +{ + Mixer *mixer = _first; + unsigned rotor_count = 0; + + while (mixer != nullptr) { + + rotor_count = mixer->get_multirotor_count(); + + if (rotor_count > 0) { break; } + + mixer = mixer -> _next; + } + + return rotor_count; +} + uint16_t MixerGroup::get_saturation_status() { diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt new file mode 100644 index 000000000000..9419f8ee205d --- /dev/null +++ b/src/lib/mixer_module/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(mixer_module mixer_module.cpp) + diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp new file mode 100644 index 000000000000..2c791776b22b --- /dev/null +++ b/src/lib/mixer_module/mixer_module.cpp @@ -0,0 +1,527 @@ +/**************************************************************************** + * + * 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 "mixer_module.hpp" + +#include +#include + +using namespace time_literals; + + +MixingOutput::MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, + bool support_esc_calibration, bool ramp_up) + : ModuleParams(&interface), + _control_subs{ + {&interface, ORB_ID(actuator_controls_0)}, + {&interface, ORB_ID(actuator_controls_1)}, + {&interface, ORB_ID(actuator_controls_2)}, + {&interface, ORB_ID(actuator_controls_3)} +}, +_scheduling_policy(scheduling_policy), +_support_esc_calibration(support_esc_calibration), +_interface(interface), +_control_latency_perf(perf_alloc(PC_ELAPSED, "control latency")) +{ + output_limit_init(&_output_limit); + _output_limit.ramp_up = ramp_up; + + /* Safely initialize armed flags */ + _armed.armed = false; + _armed.prearmed = false; + _armed.ready_to_arm = false; + _armed.lockdown = false; + _armed.force_failsafe = false; + _armed.in_esc_calibration_mode = false; + + // If there is no safety button, disable it on init. +#ifndef GPIO_BTN_SAFETY + _safety_off = true; +#endif + + px4_sem_init(&_lock, 0, 1); +} + +MixingOutput::~MixingOutput() +{ + perf_free(_control_latency_perf); + delete _mixers; + px4_sem_destroy(&_lock); +} + +void MixingOutput::printStatus() +{ + perf_print_counter(_control_latency_perf); + PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched); +} + +void MixingOutput::updateParams() +{ + ModuleParams::updateParams(); + + bool safety_disabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY); + + if (safety_disabled) { + _safety_off = true; + } + + // update mixer if we have one + if (_mixers) { + if (_param_mot_slew_max.get() <= FLT_EPSILON) { + _mixers->set_max_delta_out_once(0.f); + } + + _mixers->set_thrust_factor(_param_thr_mdl_fac.get()); + _mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get()); + } +} + +bool MixingOutput::updateSubscriptions(bool allow_wq_switch) +{ + if (_groups_subscribed == _groups_required) { + return false; + } + + // must be locked to potentially change WorkQueue + lock(); + + if (_scheduling_policy == SchedulingPolicy::Auto) { + // first clear everything + _interface.ScheduleClear(); + unregister(); + + // if subscribed to control group 0 or 1 then move to the rate_ctrl WQ + const bool sub_group_0 = (_groups_required & (1 << 0)); + const bool sub_group_1 = (_groups_required & (1 << 1)); + + if (allow_wq_switch && !_wq_switched && (sub_group_0 || sub_group_1)) { + if (_interface.ChangeWorkQeue(px4::wq_configurations::rate_ctrl)) { + // let the new WQ handle the subscribe update + _wq_switched = true; + _interface.ScheduleNow(); + unlock(); + return false; + } + } + + // register callback to all required actuator control groups + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_required & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + + if (!_control_subs[i].register_callback()) { + PX4_ERR("actuator_controls_%d register callback failed!", i); + } + } + } + + // if nothing required keep periodic schedule (so the module can update other things) + if (_groups_required == 0) { + // TODO: this might need to be configurable depending on the module + _interface.ScheduleOnInterval(100_ms); + } + } + + _groups_subscribed = _groups_required; + setMaxTopicUpdateRate(_max_topic_update_interval_us); + + PX4_DEBUG("_groups_required 0x%08x", _groups_required); + PX4_DEBUG("_groups_subscribed 0x%08x", _groups_subscribed); + + unlock(); + + return true; +} + +void MixingOutput::setMaxTopicUpdateRate(unsigned max_topic_update_interval_us) +{ + _max_topic_update_interval_us = max_topic_update_interval_us; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_subscribed & (1 << i)) { + _control_subs[i].set_interval_us(_max_topic_update_interval_us); + } + } +} + +void MixingOutput::setAllMinValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _min_value[i] = value; + } +} + +void MixingOutput::setAllMaxValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _max_value[i] = value; + } +} + +void MixingOutput::setAllFailsafeValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _failsafe_value[i] = value; + } +} + +void MixingOutput::setAllDisarmedValues(uint16_t value) +{ + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + _disarmed_value[i] = value; + } +} + +void MixingOutput::unregister() +{ + for (auto &control_sub : _control_subs) { + control_sub.unregister_callback(); + } +} + +bool MixingOutput::update() +{ + if (!_mixers) { + handleCommands(); + // do nothing until we have a valid mixer + return false; + } + + // check arming state + if (_armed_sub.update(&_armed)) { + _armed.in_esc_calibration_mode &= _support_esc_calibration; + /* Update the armed status and check that we're not locked down. + * We also need to arm throttle for the ESC calibration. */ + _throttle_armed = (_safety_off && _armed.armed && !_armed.lockdown) || (_safety_off && _armed.in_esc_calibration_mode); + } + + if (_param_mot_slew_max.get() > FLT_EPSILON) { + const hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _time_last_mix) / 1e6f, 0.0001f, 0.02f); + _time_last_mix = now; + + // maximum value the outputs of the multirotor mixer are allowed to change in this cycle + // factor 2 is needed because actuator outputs are in the range [-1,1] + const float delta_out_max = 2.0f * 1000.0f * dt / (_max_value[0] - _min_value[0]) / _param_mot_slew_max.get(); + _mixers->set_max_delta_out_once(delta_out_max); + } + + unsigned n_updates = 0; + + /* get controls for required topics */ + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_groups_subscribed & (1 << i)) { + if (_control_subs[i].copy(&_controls[i])) { + n_updates++; + } + + /* During ESC calibration, we overwrite the throttle value. */ + if (i == 0 && _armed.in_esc_calibration_mode) { + + /* Set all controls to 0 */ + memset(&_controls[i], 0, sizeof(_controls[i])); + + /* except thrust to maximum. */ + _controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f; + + /* Switch off the output limit ramp for the calibration. */ + _output_limit.state = OUTPUT_LIMIT_STATE_ON; + } + } + } + + /* do mixing */ + float outputs[MAX_ACTUATORS] {}; + const unsigned mixed_num_outputs = _mixers->mix(outputs, MAX_ACTUATORS); + + /* the output limit call takes care of out of band errors, NaN and constrains */ + uint16_t output_limited[MAX_ACTUATORS] {}; + + output_limit_calc(_throttle_armed, armNoThrottle(), mixed_num_outputs, _reverse_output_mask, + _disarmed_value, _min_value, _max_value, outputs, output_limited, &_output_limit); + + /* overwrite outputs in case of force_failsafe with _failsafe_value values */ + if (_armed.force_failsafe) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + output_limited[i] = _failsafe_value[i]; + } + } + + /* overwrite outputs in case of lockdown or parachute triggering with disarmed values */ + if (_armed.lockdown || _armed.manual_lockdown) { + for (size_t i = 0; i < mixed_num_outputs; i++) { + output_limited[i] = _disarmed_value[i]; + } + } + + bool stop_motors = true; + + if (mixed_num_outputs > 0) { + /* assume if one (here the 1.) motor is disarmed, all of them should be stopped */ + stop_motors = (output_limited[0] == _disarmed_value[0]); + } + + /* apply _param_mot_ordering */ + reorderOutputs(output_limited); + + /* now return the outputs to the driver */ + _interface.updateOutputs(stop_motors, output_limited, mixed_num_outputs, n_updates); + + + actuator_outputs_s actuator_outputs{}; + actuator_outputs.noutputs = mixed_num_outputs; + + // zero unused outputs + for (size_t i = 0; i < mixed_num_outputs; ++i) { + actuator_outputs.output[i] = output_limited[i]; + } + + actuator_outputs.timestamp = hrt_absolute_time(); + _outputs_pub.publish(actuator_outputs); + + /* publish mixer status */ + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = _mixers->get_saturation_status(); + + if (saturation_status.flags.valid) { + multirotor_motor_limits_s motor_limits; + motor_limits.timestamp = actuator_outputs.timestamp; + motor_limits.saturation_status = saturation_status.value; + + _to_mixer_status.publish(motor_limits); + } + + // use first valid timestamp_sample for latency tracking + for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + const bool required = _groups_required & (1 << i); + const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample; + + if (required && (timestamp_sample > 0)) { + perf_set_elapsed(_control_latency_perf, actuator_outputs.timestamp - timestamp_sample); + break; + } + } + + // check safety button state + if (_safety_sub.updated()) { + safety_s safety; + + if (_safety_sub.copy(&safety)) { + _safety_off = !safety.safety_switch_available || safety.safety_off; + } + } + + handleCommands(); + + return true; +} + +void +MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) +{ + if (MAX_ACTUATORS < 4) { + return; + } + + if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { + /* + * Betaflight default motor ordering: + * 4 2 + * ^ + * 3 1 + */ + const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] }; + values[0] = value_tmp[3]; + values[1] = value_tmp[0]; + values[2] = value_tmp[1]; + values[3] = value_tmp[2]; + } + + /* else: PX4, no need to reorder + * 3 1 + * ^ + * 2 4 + */ +} + +int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) +{ + const MixingOutput *output = (const MixingOutput *)handle; + + input = output->_controls[control_group].control[control_index]; + + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + + } else if (input < -1.0f) { + input = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ + if (output->_output_limit.state == OUTPUT_LIMIT_STATE_RAMP) { + if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* limit the throttle output to zero during motor spinup, + * as the motors cannot follow any demand yet + */ + input = 0.0f; + } + } + + /* throttle not arming - mark throttle input as invalid */ + if (output->armNoThrottle() && !output->_armed.in_esc_calibration_mode) { + if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || + control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN; + } + } + + return 0; +} + +void MixingOutput::resetMixer() +{ + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + } +} + +int MixingOutput::loadMixer(const char *buf, unsigned len) +{ + if (_mixers == nullptr) { + _mixers = new MixerGroup(controlCallback, (uintptr_t)this); + } + + if (_mixers == nullptr) { + _groups_required = 0; + return -ENOMEM; + } + + int ret = _mixers->load_from_buf(buf, len); + + if (ret != 0) { + PX4_ERR("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + _groups_required = 0; + return ret; + } + + _mixers->groups_required(_groups_required); + PX4_DEBUG("loaded mixers \n%s\n", buf); + + updateParams(); + return ret; +} + +void MixingOutput::handleCommands() +{ + if (_command.command.load() == Command::Type::None) { + return; + } + + switch (_command.command.load()) { + case Command::Type::loadMixer: + _command.result = loadMixer(_command.mixer_buf, _command.mixer_buf_length); + break; + + case Command::Type::resetMixer: + resetMixer(); + _command.result = 0; + break; + + default: + break; + } + + // mark as done + _command.command.store(Command::Type::None); +} + +void MixingOutput::resetMixerThreadSafe() +{ + if (_command.command.load() != Command::Type::None) { + // Cannot happen, because we expect only one other thread to call this. + // But as a safety precaution we return here. + PX4_ERR("Command not None"); + return; + } + + lock(); + + _command.command.store(Command::Type::resetMixer); + + _interface.ScheduleNow(); + + unlock(); + + // wait until processed + while (_command.command.load() != Command::Type::None) { + usleep(1000); + } + +} + +int MixingOutput::loadMixerThreadSafe(const char *buf, unsigned len) +{ + if (_command.command.load() != Command::Type::None) { + // Cannot happen, because we expect only one other thread to call this. + // But as a safety precaution we return here. + PX4_ERR("Command not None"); + return -1; + } + + lock(); + + _command.mixer_buf = buf; + _command.mixer_buf_length = len; + _command.command.store(Command::Type::loadMixer); + + _interface.ScheduleNow(); + + unlock(); + + // wait until processed + while (_command.command.load() != Command::Type::None) { + usleep(1000); + } + + return _command.result; +} + diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp new file mode 100644 index 000000000000..46f010c69095 --- /dev/null +++ b/src/lib/mixer_module/mixer_module.hpp @@ -0,0 +1,240 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * @class OutputModuleInterface + * Base class for an output module. + */ +class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams +{ +public: + static constexpr int MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; + + OutputModuleInterface(const px4::wq_config_t &config) + : px4::ScheduledWorkItem(config), ModuleParams(nullptr) {} + + virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) = 0; +}; + +/** + * @class MixingOutput + * This handles the mixing, arming/disarming and all subscriptions required for that. + * + * It can also drive the scheduling of the OutputModuleInterface (via uORB callbacks + * to reduce output latency). + */ +class MixingOutput : public ModuleParams +{ +public: + static constexpr int MAX_ACTUATORS = OutputModuleInterface::MAX_ACTUATORS; + + enum class SchedulingPolicy { + Disabled, ///< Do not drive scheduling (the module needs to call ScheduleOnInterval() for example) + Auto ///< Drive scheduling based on subscribed actuator controls topics (via uORB callbacks) + }; + + /** + * Contructor + * @param interface Parent module for scheduling, parameter updates and callbacks + * @param scheduling_policy + * @param support_esc_calibration true if the output module supports ESC calibration via max, then min setting + * @param ramp_up true if motor ramp up from disarmed to min upon arming is wanted + */ + MixingOutput(OutputModuleInterface &interface, SchedulingPolicy scheduling_policy, + bool support_esc_calibration, bool ramp_up = true); + + ~MixingOutput(); + + void printStatus(); + + /** + * Call this regularly from Run(). It will call interface.updateOutputs(). + * @return true if outputs were updated + */ + bool update(); + + /** + * Check for subscription updates (e.g. after a mixer is loaded). + * Call this at the very end of Run() if allow_wq_switch + * @param allow_wq_switch if true + * @return true if subscriptions got changed + */ + bool updateSubscriptions(bool allow_wq_switch); + + /** + * unregister uORB subscription callbacks + */ + void unregister(); + + void setMaxTopicUpdateRate(unsigned max_topic_update_interval_us); + + /** + * Reset (unload) the complete mixer, called from another thread. + * This is thread-safe, as long as only one other thread at a time calls this. + */ + void resetMixerThreadSafe(); + + void resetMixer(); + + /** + * Load (append) a new mixer from a buffer, called from another thread. + * This is thread-safe, as long as only one other thread at a time calls this. + * @return 0 on success, <0 error otherwise + */ + int loadMixerThreadSafe(const char *buf, unsigned len); + + int loadMixer(const char *buf, unsigned len); + + const actuator_armed_s &armed() const { return _armed; } + + MixerGroup *mixers() const { return _mixers; } + + void setAllFailsafeValues(uint16_t value); + void setAllDisarmedValues(uint16_t value); + void setAllMinValues(uint16_t value); + void setAllMaxValues(uint16_t value); + + uint16_t &reverseOutputMask() { return _reverse_output_mask; } + uint16_t &failsafeValue(int index) { return _failsafe_value[index]; } + /** Disarmed values: if ramp_up is true, then disarmedValue < minValue needs to hold */ + uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } + uint16_t &minValue(int index) { return _min_value[index]; } + uint16_t &maxValue(int index) { return _max_value[index]; } + +protected: + void updateParams() override; + +private: + void handleCommands(); + + bool armNoThrottle() const + { + return (_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode; + } + static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); + + enum class MotorOrdering : int32_t { + PX4 = 0, + Betaflight = 1 + }; + + struct Command { + enum class Type : int { + None, + resetMixer, + loadMixer + }; + px4::atomic command{Type::None}; + const char *mixer_buf; + unsigned mixer_buf_length; + int result; + }; + Command _command; ///< incoming commands (from another thread) + + /** + * Reorder outputs according to _param_mot_ordering + * @param values values to reorder + */ + inline void reorderOutputs(uint16_t values[MAX_ACTUATORS]); + + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + void unlock() { px4_sem_post(&_lock); } + + px4_sem_t _lock; /**< lock to protect access to work queue changes (includes ScheduleNow calls from another thread) */ + + uint16_t _failsafe_value[MAX_ACTUATORS] {}; + uint16_t _disarmed_value[MAX_ACTUATORS] {}; + uint16_t _min_value[MAX_ACTUATORS] {}; + uint16_t _max_value[MAX_ACTUATORS] {}; + uint16_t _reverse_output_mask{0}; ///< reverses the interval [min, max] -> [max, min], NOT motor direction + output_limit_t _output_limit; + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _safety_sub{ORB_ID(safety)}; + uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + + uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs), ORB_PRIO_DEFAULT}; + uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits), ORB_PRIO_DEFAULT}; ///< mixer status flags + + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; + actuator_armed_s _armed{}; + + hrt_abstime _time_last_mix{0}; + unsigned _max_topic_update_interval_us{0}; ///< max _control_subs topic update interval (0=unlimited) + + bool _safety_off{false}; ///< State of the safety button from the subscribed _safety_sub topic + bool _throttle_armed{false}; + + MixerGroup *_mixers{nullptr}; + uint32_t _groups_required{0}; + uint32_t _groups_subscribed{1u << 31}; ///< initialize to a different value than _groups_required and outside of (1 << NUM_ACTUATOR_CONTROL_GROUPS) + + const SchedulingPolicy _scheduling_policy; + const bool _support_esc_calibration; + + bool _wq_switched{false}; + + OutputModuleInterface &_interface; + + perf_counter_t _control_latency_perf; + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode, ///< multicopter air-mode + (ParamFloat) _param_mot_slew_max, + (ParamFloat) _param_thr_mdl_fac, ///< thrust to pwm modelling factor + (ParamInt) _param_mot_ordering + ) +}; + diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c new file mode 100644 index 000000000000..26acd17694cf --- /dev/null +++ b/src/lib/mixer_module/params.c @@ -0,0 +1,18 @@ + +/** + * Multicopter air-mode + * + * The air-mode enables the mixer to increase the total thrust of the multirotor + * in order to keep attitude and rate control even at low and high throttle. + * + * This function should be disabled during tuning as it will help the controller + * to diverge if the closed-loop is unstable (i.e. the vehicle is not tuned yet). + * + * Enabling air-mode for yaw requires the use of an arming switch. + * + * @value 0 Disabled + * @value 1 Roll/Pitch + * @value 2 Roll/Pitch/Yaw + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_INT32(MC_AIRMODE, 0); diff --git a/src/lib/pwm_limit/CMakeLists.txt b/src/lib/output_limit/CMakeLists.txt similarity index 97% rename from src/lib/pwm_limit/CMakeLists.txt rename to src/lib/output_limit/CMakeLists.txt index 3e2c83f2f41e..851872e10df6 100644 --- a/src/lib/pwm_limit/CMakeLists.txt +++ b/src/lib/output_limit/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(pwm_limit pwm_limit.cpp) +px4_add_library(output_limit output_limit.cpp) diff --git a/src/lib/pwm_limit/pwm_limit.cpp b/src/lib/output_limit/output_limit.cpp similarity index 63% rename from src/lib/pwm_limit/pwm_limit.cpp rename to src/lib/output_limit/output_limit.cpp index b2b9ebd5861f..781c069b5265 100644 --- a/src/lib/pwm_limit/pwm_limit.cpp +++ b/src/lib/output_limit/output_limit.cpp @@ -31,16 +31,7 @@ * ****************************************************************************/ -/** - * @file pwm_limit.c - * - * Library for PWM output limiting - * - * @author Julian Oes - * @author Lorenz Meier - */ - -#include "pwm_limit.h" +#include "output_limit.h" #include #include @@ -50,20 +41,21 @@ #define PROGRESS_INT_SCALING 10000 -void pwm_limit_init(pwm_limit_t *limit) +void output_limit_init(output_limit_t *limit) { - limit->state = PWM_LIMIT_STATE_INIT; + limit->state = OUTPUT_LIMIT_STATE_INIT; limit->time_armed = 0; + limit->ramp_up = true; } -void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, + const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output, + const float *output, uint16_t *effective_output, output_limit_t *limit) { /* first evaluate state changes */ switch (limit->state) { - case PWM_LIMIT_STATE_INIT: + case OUTPUT_LIMIT_STATE_INIT: if (armed) { @@ -73,15 +65,20 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c } if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } } break; - case PWM_LIMIT_STATE_OFF: + case OUTPUT_LIMIT_STATE_OFF: if (armed) { - limit->state = PWM_LIMIT_STATE_RAMP; + if (limit->ramp_up) { + limit->state = OUTPUT_LIMIT_STATE_RAMP; + + } else { + limit->state = OUTPUT_LIMIT_STATE_ON; + } /* reset arming time, used for ramp timing */ limit->time_armed = hrt_absolute_time(); @@ -89,19 +86,19 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c break; - case PWM_LIMIT_STATE_RAMP: + case OUTPUT_LIMIT_STATE_RAMP: if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { - limit->state = PWM_LIMIT_STATE_ON; + limit->state = OUTPUT_LIMIT_STATE_ON; } break; - case PWM_LIMIT_STATE_ON: + case OUTPUT_LIMIT_STATE_ON: if (!armed) { - limit->state = PWM_LIMIT_STATE_OFF; + limit->state = OUTPUT_LIMIT_STATE_OFF; } break; @@ -120,22 +117,22 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c unsigned local_limit_state = limit->state; if (pre_armed) { - local_limit_state = PWM_LIMIT_STATE_ON; + local_limit_state = OUTPUT_LIMIT_STATE_ON; } unsigned progress; - /* then set effective_pwm based on state */ + /* then set effective_output based on state */ switch (local_limit_state) { - case PWM_LIMIT_STATE_OFF: - case PWM_LIMIT_STATE_INIT: + case OUTPUT_LIMIT_STATE_OFF: + case OUTPUT_LIMIT_STATE_INIT: for (unsigned i = 0; i < num_channels; i++) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; } break; - case PWM_LIMIT_STATE_RAMP: { + case OUTPUT_LIMIT_STATE_RAMP: { hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; @@ -150,49 +147,49 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* check for invalid / disabled channels */ if (!PX4_ISFINITE(control_value)) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; continue; } - uint16_t ramp_min_pwm; + uint16_t ramp_min_output; - /* if a disarmed pwm value was set, blend between disarmed and min */ - if (disarmed_pwm[i] > 0) { + /* if a disarmed output value was set, blend between disarmed and min */ + if (disarmed_output[i] > 0) { /* safeguard against overflows */ - unsigned disarmed = disarmed_pwm[i]; + unsigned disarmed = disarmed_output[i]; - if (disarmed > min_pwm[i]) { - disarmed = min_pwm[i]; + if (disarmed > min_output[i]) { + disarmed = min_output[i]; } - unsigned disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; + unsigned disarmed_min_diff = min_output[i] - disarmed; + ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; } else { - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; + /* no disarmed output value set, choose min output */ + ramp_min_output = min_output[i]; } if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2; + effective_output[i] = control_value * (max_output[i] - ramp_min_output) / 2 + (max_output[i] + ramp_min_output) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < ramp_min_pwm) { - effective_pwm[i] = ramp_min_pwm; + if (effective_output[i] < ramp_min_output) { + effective_output[i] = ramp_min_output; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; + } else if (effective_output[i] > max_output[i]) { + effective_output[i] = max_output[i]; } } } break; - case PWM_LIMIT_STATE_ON: + case OUTPUT_LIMIT_STATE_ON: for (unsigned i = 0; i < num_channels; i++) { @@ -200,7 +197,7 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c /* check for invalid / disabled channels */ if (!PX4_ISFINITE(control_value)) { - effective_pwm[i] = disarmed_pwm[i]; + effective_output[i] = disarmed_output[i]; continue; } @@ -208,14 +205,14 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 + (max_output[i] + min_output[i]) / 2; /* last line of defense against invalid inputs */ - if (effective_pwm[i] < min_pwm[i]) { - effective_pwm[i] = min_pwm[i]; + if (effective_output[i] < min_output[i]) { + effective_output[i] = min_output[i]; - } else if (effective_pwm[i] > max_pwm[i]) { - effective_pwm[i] = max_pwm[i]; + } else if (effective_output[i] > max_output[i]) { + effective_output[i] = max_output[i]; } } diff --git a/src/lib/pwm_limit/pwm_limit.h b/src/lib/output_limit/output_limit.h similarity index 72% rename from src/lib/pwm_limit/pwm_limit.h rename to src/lib/output_limit/output_limit.h index 5347f9b5eb47..be9d969bd1be 100644 --- a/src/lib/pwm_limit/pwm_limit.h +++ b/src/lib/output_limit/output_limit.h @@ -32,15 +32,14 @@ ****************************************************************************/ /** - * @file pwm_limit.c + * @file output_limit.h * - * Library for PWM output limiting + * Library for output limiting (PWM for example) * * @author Julian Oes */ -#ifndef PWM_LIMIT_H_ -#define PWM_LIMIT_H_ +#pragma once #include #include @@ -49,7 +48,7 @@ __BEGIN_DECLS /* * time for the ESCs to initialize - * (this is not actually needed if PWM is sent right after boot) + * (this is not actually needed if the signal is sent right after boot) */ #define INIT_TIME_US 50000 /* @@ -57,25 +56,25 @@ __BEGIN_DECLS */ #define RAMP_TIME_US 500000 -enum pwm_limit_state { - PWM_LIMIT_STATE_OFF = 0, - PWM_LIMIT_STATE_INIT, - PWM_LIMIT_STATE_RAMP, - PWM_LIMIT_STATE_ON +enum output_limit_state { + OUTPUT_LIMIT_STATE_OFF = 0, + OUTPUT_LIMIT_STATE_INIT, + OUTPUT_LIMIT_STATE_RAMP, + OUTPUT_LIMIT_STATE_ON }; typedef struct { - enum pwm_limit_state state; + enum output_limit_state state; uint64_t time_armed; -} pwm_limit_t; + bool ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming +} output_limit_t; -__EXPORT void pwm_limit_init(pwm_limit_t *limit); +__EXPORT void output_limit_init(output_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, - const uint16_t reverse_mask, const uint16_t *disarmed_pwm, - const uint16_t *min_pwm, const uint16_t *max_pwm, - const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, + const uint16_t reverse_mask, const uint16_t *disarmed_output, + const uint16_t *min_output, const uint16_t *max_output, + const float *output, uint16_t *effective_output, output_limit_t *limit); __END_DECLS -#endif /* PWM_LIMIT_H_ */ diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index c31fad719e45..6ee025585670 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -155,10 +155,12 @@ if (NOT "${PX4_BOARD}" MATCHES "px4_io") -Wno-sign-compare # TODO: fix and enable ) else() - add_library(parameters ${PX4_SOURCE_DIR}/src/platforms/empty.c) + add_library(parameters ${PX4_SOURCE_DIR}/platforms/common/empty.c) endif() add_dependencies(parameters prebuild_targets) if(${PX4_PLATFORM} STREQUAL "nuttx") target_link_libraries(parameters PRIVATE flashparams tinybson) endif() + +px4_add_functional_gtest(SRC ParameterTest.cpp LINKLIBS parameters) diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp new file mode 100644 index 000000000000..1e9bec0336d8 --- /dev/null +++ b/src/lib/parameters/ParameterTest.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 + +class ParameterTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_reset_all(); + } +}; + + +TEST_F(ParameterTest, testParamReadWrite) +{ + // GIVEN a parameter handle + param_t param = param_handle(px4::params::MPC_COL_PREV_D); + + // WHEN: we get the parameter + float value = -999.f; + int status = param_get(param, &value); + + // THEN it should be successful and have the default value + EXPECT_EQ(0, status); + EXPECT_EQ(-1, value); + + // WHEN: we set the parameter + value = 42.f; + status = param_set(param, &value); + + // THEN: it should be successful + EXPECT_EQ(0, status); + + // WHEN: we get the parameter again + float value2 = -1999.f; + status = param_get(param, &value2); + + // THEN: it should be exactly the value we set + EXPECT_EQ(0, status); + EXPECT_EQ(42.f, value2); +} + + +TEST_F(ParameterTest, testUorbSendReceive) +{ + // GIVEN: a uOrb message + obstacle_distance_s message; + memset(&message, 0xDEAD, sizeof(message)); + message.min_distance = 1.f; + message.max_distance = 10.f; + + // AND: a subscriber + uORB::SubscriptionData sub_obstacle_distance{ORB_ID(obstacle_distance)}; + + // WHEN we send the message + orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); + ASSERT_TRUE(obstacle_distance_pub != nullptr); + + // THEN: the subscriber should receive the message + sub_obstacle_distance.update(); + const obstacle_distance_s &obstacle_distance = sub_obstacle_distance.get(); + + // AND: the values we got should be the same + EXPECT_EQ(message.timestamp, obstacle_distance.timestamp); + EXPECT_EQ(message.min_distance, obstacle_distance.min_distance); + EXPECT_EQ(message.max_distance, obstacle_distance.max_distance); + + // AND: all the bytes should be equal + EXPECT_EQ(0, memcmp(&message, &obstacle_distance, sizeof(message))); +} diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index 98634ae4288a..06d78ac51f03 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -4,7 +4,10 @@ class MarkdownTablesOutput(): def __init__(self, groups): result = ("# Parameter Reference\n" - "> **Note** **This list is auto-generated from the source code** (using `make parameters_metadata`) and contains the most recent parameter documentation.\n" + "> **Note** **This documentation was auto-generated from the source code for this PX4 version** (using `make parameters_metadata`).\n" + "\n" + "\n" + "> **Note** If a listed parameter is missing from the Firmware see: [Finding/Updating Parameters](http://docs.px4.io/master/en/advanced_config/parameters.html#missing).\n" "\n") for group in groups: result += '## %s\n\n' % group.GetName() diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt index 62a3b1161920..4975dfcb123c 100644 --- a/src/lib/systemlib/CMakeLists.txt +++ b/src/lib/systemlib/CMakeLists.txt @@ -35,7 +35,7 @@ set(SRCS conversions.c cpuload.c crc.c - mavlink_log.c + mavlink_log.cpp otp.c ) diff --git a/src/lib/systemlib/mavlink_log.c b/src/lib/systemlib/mavlink_log.cpp similarity index 90% rename from src/lib/systemlib/mavlink_log.c rename to src/lib/systemlib/mavlink_log.cpp index a83a6d680a82..142be78ced94 100644 --- a/src/lib/systemlib/mavlink_log.c +++ b/src/lib/systemlib/mavlink_log.cpp @@ -48,9 +48,6 @@ #include #include "mavlink_log.h" -#define MAVLINK_LOG_QUEUE_SIZE 5 - - __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, const char *fmt, ...) { // TODO: add compile check for maxlen @@ -59,31 +56,23 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con return; } - if (mavlink_log_pub == NULL) { + if (mavlink_log_pub == nullptr) { return; } - struct mavlink_log_s log_msg; - + mavlink_log_s log_msg; log_msg.severity = severity; - log_msg.timestamp = hrt_absolute_time(); va_list ap; - va_start(ap, fmt); - vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap); - va_end(ap); - if (*mavlink_log_pub != NULL) { + if (*mavlink_log_pub != nullptr) { orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); } else { - *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), - &log_msg, - MAVLINK_LOG_QUEUE_SIZE); + *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); } } - diff --git a/src/lib/version/px_update_git_header.py b/src/lib/version/px_update_git_header.py index d13cb78c3061..423fa8692c6a 100755 --- a/src/lib/version/px_update_git_header.py +++ b/src/lib/version/px_update_git_header.py @@ -47,7 +47,7 @@ # remove optional --g at the end (in case we are not on a tagged commit) git_tag_test = re.sub(r'-[0-9]+-g[0-9a-fA-F]+$', '', git_tag_test) # now check the version format - m = re.match(r'v([0-9]+)\.([0-9]+)\.[0-9]+(rc[0-9]+)?(-[0-9]+\.[0-9]+\.[0-9]+)?$', git_tag_test) + m = re.match(r'v([0-9]+)\.([0-9]+)\.[0-9]+([-]?rc[0-9]+)?(-beta[0-9]+)?(-[0-9]+\.[0-9]+\.[0-9]+)?$', git_tag_test) if m: # format matches, check the major and minor numbers major = int(m.group(1)) @@ -63,10 +63,12 @@ print("Error: the git tag '{:}' does not match the expected format.".format(git_tag_test)) print("") print("The expected format is 'v[-]'") - print(" : v..[rc]") + print(" : v..[-rc|-beta]") print(" : ..") print("Examples:") - print(" v1.9.0rc3") + print(" v1.9.0rc3 (deprecated)") + print(" v1.9.0-rc3 (preferred)") + print(" v1.9.0-beta1") print(" v1.9.0-1.0.0") print("See also https://dev.px4.io/master/en/setup/building_px4.html#firmware_version") print("") diff --git a/src/lib/version/version.h b/src/lib/version/version.h index f13cca445348..3c02e89eeb13 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,8 +89,9 @@ const char *px4_build_uri(void); * Convert a version tag string to a number * @param tag version tag in one of the following forms: * - vendor: v1.4.0-0.2.0 - * - dev: v1.4.0rc3-7-g7e282f57 - * - rc: v1.4.0rc4 + * - dev: v1.4.0-rc3-7-g7e282f57 + * - rc: v1.4.0-rc4 + * - beta: v1.4.0-beta1 * - release: v1.4.0 * - linux: 7.9.3 * @return version in the form 0xAABBCCTT (AA: Major, BB: Minor, CC: Patch, TT Type @see FIRMWARE_TYPE) @@ -108,8 +108,9 @@ __EXPORT uint32_t px4_firmware_version(void); * Convert a version tag string to a vendor version number * @param tag version tag in one of the following forms: * - vendor: v1.4.0-0.2.0 - * - dev: v1.4.0rc3-7-g7e282f57 - * - rc: v1.4.0rc4 + * - dev: v1.4.0-rc3-7-g7e282f57 + * - rc: v1.4.0-rc4 + * - beta: v1.4.0-beta1 * - release: v1.4.0 * - linux: 7.9.3 * @return version in the form 0xAABBCCTT (AA: Major, BB: Minor, CC: Patch, TT Type @see FIRMWARE_TYPE) diff --git a/src/modules/wind_estimator/CMakeLists.txt b/src/modules/airspeed_selector/CMakeLists.txt similarity index 93% rename from src/modules/wind_estimator/CMakeLists.txt rename to src/modules/airspeed_selector/CMakeLists.txt index 3f6bff2d4f2b..6c2ad20cf440 100644 --- a/src/modules/wind_estimator/CMakeLists.txt +++ b/src/modules/airspeed_selector/CMakeLists.txt @@ -31,13 +31,12 @@ # ############################################################################ px4_add_module( - MODULE modules__wind_estimator - MAIN wind_estimator - INCLUDES - ${PX4_SOURCE_DIR}/src/lib/ecl + MODULE modules__airspeed_selector + MAIN airspeed_selector SRCS - wind_estimator_main.cpp + airspeed_selector_main.cpp DEPENDS git_ecl ecl_airdata + AirspeedValidator ) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp new file mode 100644 index 000000000000..87adfef289ff --- /dev/null +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -0,0 +1,592 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ + +using matrix::Dcmf; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; + +class AirspeedModule : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + + AirspeedModule(); + + ~AirspeedModule(); + + /** @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); + + /* run the main loop */ + void Run() override; + + int print_status() override; + +private: + static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ + + uORB::Publication _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ + uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ + orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ + + uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + + estimator_status_s _estimator_status {}; + parameter_update_s _parameter_update {}; + vehicle_acceleration_s _accel {}; + vehicle_air_data_s _vehicle_air_data {}; + vehicle_attitude_s _vehicle_attitude {}; + vehicle_land_detected_s _vehicle_land_detected {}; + vehicle_local_position_s _vehicle_local_position {}; + vehicle_status_s _vehicle_status {}; + vtol_vehicle_status_s _vtol_vehicle_status {}; + + WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ + wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ + + int _airspeed_sub[MAX_NUM_AIRSPEED_SENSORS] {}; /**< raw airspeed topics subscriptions. Max 3 airspeeds sensors. */ + int _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ + AirspeedValidator *_airspeed_validator{nullptr}; /**< airspeedValidator instances (one for each sensor, assigned dynamically during startup) */ + + int _valid_airspeed_index{-1}; /**< index of currently chosen (valid) airspeed sensor */ + int _prev_airspeed_index{-1}; /**< previously chosen airspeed sensor index */ + bool _initialized{false}; /**< module initialized*/ + bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */ + bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ + float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ + float _ground_minus_wind_EAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */ + + bool _scale_estimation_previously_on{false}; /**< scale_estimation was on in the last cycle */ + + perf_counter_t _perf_elapsed{}; + perf_counter_t _perf_interval{}; + + + DEFINE_PARAMETERS( + (ParamFloat) _param_west_w_p_noise, + (ParamFloat) _param_west_sc_p_noise, + (ParamFloat) _param_west_tas_noise, + (ParamFloat) _param_west_beta_noise, + (ParamInt) _param_west_tas_gate, + (ParamInt) _param_west_beta_gate, + (ParamInt) _param_west_scale_estimation_on, + (ParamFloat) _param_west_airspeed_scale, + + + (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ + (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ + (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ + (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ + (ParamFloat) _airspeed_stall /**< stall speed*/ + ) + + int start(); + void update_params(); /**< update parameters */ + void poll_topics(); /**< poll all topics required beside airspeed (e.g. current temperature) */ + void update_wind_estimator_sideslip(); /**< update the wind estimator instance only fusing sideslip */ + void update_ground_minus_wind_airspeed(); /**< update airspeed estimate based on groundspeed minus windspeed */ + void select_airspeed_and_publish(); /**< select airspeed sensor (or groundspeed-windspeed) */ + void publish_wind_estimates(); /**< publish wind estimator states (from all wind estimators running) */ + +}; + +AirspeedModule::AirspeedModule(): + ModuleParams(nullptr), + ScheduledWorkItem(px4::wq_configurations::lp_default) +{ + // initialise parameters + update_params(); + + _perf_elapsed = perf_alloc_once(PC_ELAPSED, "airspeed_selector elapsed"); + _perf_interval = perf_alloc_once(PC_INTERVAL, "airspeed_selector interval"); +} + +AirspeedModule::~AirspeedModule() +{ + ScheduleClear(); + + perf_free(_perf_elapsed); + perf_free(_perf_interval); + + if (_airspeed_validator != nullptr) { + delete[] _airspeed_validator; + } +} + +int +AirspeedModule::task_spawn(int argc, char *argv[]) +{ + AirspeedModule *dev = new AirspeedModule(); + + // check if the trampoline is called for the first time + if (!dev) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(dev); + + if (dev) { + dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); + _task_id = task_id_is_work_queue; + return PX4_OK; + } + + return PX4_ERROR; +} + +void +AirspeedModule::Run() +{ + perf_count(_perf_interval); + perf_begin(_perf_elapsed); + + /* the first time we run through here, initialize N airspeedValidator + * instances (N = number of airspeed sensors detected) + */ + if (!_initialized) { + for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) { + if (orb_exists(ORB_ID(airspeed), i) != 0) { + continue; + } + + _number_of_airspeed_sensors = i + 1; + } + + _airspeed_validator = new AirspeedValidator[_number_of_airspeed_sensors]; + + if (_number_of_airspeed_sensors > 0) { + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + _airspeed_sub[i] = orb_subscribe_multi(ORB_ID(airspeed), i); + _valid_airspeed_index = 0; // set index to first sensor + _prev_airspeed_index = 0; // set index to first sensor + } + } + + _initialized = true; + } + + parameter_update_s update; + + if (_param_sub.update(&update)) { + update_params(); + } + + poll_topics(); + update_wind_estimator_sideslip(); + update_ground_minus_wind_airspeed(); + + if (_airspeed_validator != nullptr) { + + bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + bool fixed_wing = !_vtol_vehicle_status.vtol_in_rw_mode; + bool in_air = !_vehicle_land_detected.landed; + + /* Prepare data for airspeed_validator */ + struct airspeed_validator_update_data input_data = {}; + input_data.timestamp = hrt_absolute_time(); + input_data.lpos_vx = _vehicle_local_position.vx; + input_data.lpos_vy = _vehicle_local_position.vy; + input_data.lpos_vz = _vehicle_local_position.vz; + input_data.lpos_valid = _vehicle_local_position_valid; + input_data.lpos_evh = _vehicle_local_position.evh; + input_data.lpos_evv = _vehicle_local_position.evv; + input_data.att_q[0] = _vehicle_attitude.q[0]; + input_data.att_q[1] = _vehicle_attitude.q[1]; + input_data.att_q[2] = _vehicle_attitude.q[2]; + input_data.att_q[3] = _vehicle_attitude.q[3]; + input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa; + input_data.accel_z = _accel.xyz[2]; + input_data.vel_test_ratio = _estimator_status.vel_test_ratio; + input_data.mag_test_ratio = _estimator_status.mag_test_ratio; + + /* iterate through all airspeed sensors, poll new data from them and update their validators */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + + /* poll airspeed data */ + airspeed_s airspeed_raw = {}; + orb_copy(ORB_ID(airspeed), _airspeed_sub[i], &airspeed_raw); // poll raw airspeed topic of the i-th sensor + input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; + input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; + input_data.airspeed_timestamp = airspeed_raw.timestamp; + input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + + /* update in_fixed_wing_flight for the current airspeed sensor validator */ + /* takeoff situation is active from start till one of the sensors' IAS or groundspeed_EAS is above stall speed */ + if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_EAS > _airspeed_stall.get()) { + _in_takeoff_situation = false; + } + + /* reset takeoff_situation to true when not in air or not in fixed-wing mode */ + if (!in_air || !fixed_wing) { + _in_takeoff_situation = true; + } + + input_data.in_fixed_wing_flight = (armed && fixed_wing && in_air && !_in_takeoff_situation); + + /* push input data into airspeed validator */ + _airspeed_validator[i].update_airspeed_validator(input_data); + + } + } + + select_airspeed_and_publish(); + + perf_end(_perf_elapsed); + + if (should_exit()) { + exit_and_cleanup(); + } +} + + +void AirspeedModule::update_params() +{ + updateParams(); + + /* update wind estimator (sideslip fusion only) parameters */ + _wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get()); + _wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); + _wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get()); + _wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get()); + _wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get()); + _wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get()); + + /* update airspeedValidator parameters */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + _airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get()); + _airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get()); + _airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get()); + _airspeed_validator[i].set_wind_estimator_beta_gate(_param_west_beta_gate.get()); + _airspeed_validator[i].set_wind_estimator_scale_estimation_on(_param_west_scale_estimation_on.get()); + + /* Only apply manual entered airspeed scale to first airspeed measurement */ + _airspeed_validator[0].set_airspeed_scale(_param_west_airspeed_scale.get()); + + _airspeed_validator[i].set_tas_innov_threshold(_tas_innov_threshold.get()); + _airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get()); + _airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get()); + _airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get()); + _airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get()); + } + + /* when airspeed scale estimation is turned on and the airspeed is valid, then set the scale inside the wind estimator to -1 such that it starts to estimate it */ + if (!_scale_estimation_previously_on && _param_west_scale_estimation_on.get()) { + if (_valid_airspeed_index >= 0) { + _airspeed_validator[0].set_airspeed_scale( + -1.0f); // set it to a negative value to start estimation inside wind estimator + + } else { + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); + _param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on + _param_west_scale_estimation_on.commit_no_notification(); + } + + /* If one sensor is valid and we switched out of scale estimation, then publish message and change the value of param ARSP_ARSP_SCALE */ + + } else if (_scale_estimation_previously_on && !_param_west_scale_estimation_on.get()) { + if (_valid_airspeed_index >= 0) { + + _param_west_airspeed_scale.set(_airspeed_validator[_valid_airspeed_index].get_EAS_scale()); + _param_west_airspeed_scale.commit_no_notification(); + _airspeed_validator[_valid_airspeed_index].set_airspeed_scale(_param_west_airspeed_scale.get()); + + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ARSP_ARSP_SCALE): %0.2f", + (double)_airspeed_validator[_valid_airspeed_index].get_EAS_scale()); + + } else { + mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); + } + } + + _scale_estimation_previously_on = _param_west_scale_estimation_on.get(); + +} + +void AirspeedModule::poll_topics() +{ + _estimator_status_sub.update(&_estimator_status); + _vehicle_acceleration_sub.update(&_accel); + _vehicle_air_data_sub.update(&_vehicle_air_data); + _vehicle_attitude_sub.update(&_vehicle_attitude); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _vehicle_status_sub.update(&_vehicle_status); + _vtol_vehicle_status_sub.update(&_vtol_vehicle_status); + _vehicle_local_position_sub.update(&_vehicle_local_position); + + _vehicle_local_position_valid = (hrt_absolute_time() - _vehicle_local_position.timestamp < 1_s) + && (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid; + + + +} + +void AirspeedModule::update_wind_estimator_sideslip() +{ + bool att_valid = true; // TODO: check if attitude is valid + const hrt_abstime time_now_usec = hrt_absolute_time(); + + /* update wind and airspeed estimator */ + _wind_estimator_sideslip.update(time_now_usec); + + if (_vehicle_local_position_valid && att_valid) { + Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); + Quatf q(_vehicle_attitude.q); + + /* sideslip fusion */ + _wind_estimator_sideslip.fuse_beta(time_now_usec, vI, q); + } + + /* fill message for publishing later */ + _wind_estimate_sideslip.timestamp = time_now_usec; + float wind[2]; + _wind_estimator_sideslip.get_wind(wind); + _wind_estimate_sideslip.windspeed_north = wind[0]; + _wind_estimate_sideslip.windspeed_east = wind[1]; + float wind_cov[2]; + _wind_estimator_sideslip.get_wind_var(wind_cov); + _wind_estimate_sideslip.variance_north = wind_cov[0]; + _wind_estimate_sideslip.variance_east = wind_cov[1]; + _wind_estimate_sideslip.tas_innov = _wind_estimator_sideslip.get_tas_innov(); + _wind_estimate_sideslip.tas_innov_var = _wind_estimator_sideslip.get_tas_innov_var(); + _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); + _wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var(); + _wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale(); +} + +void AirspeedModule::update_ground_minus_wind_airspeed() +{ + /* calculate airspeed estimate based on groundspeed-windspeed to use as fallback */ + float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north; + float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east; + float TAS_down = _vehicle_local_position.vz; // no wind estimate in z + _ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down); + _ground_minus_wind_EAS = calc_EAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa, + _vehicle_air_data.baro_temp_celcius); +} + + +void AirspeedModule::select_airspeed_and_publish() +{ + /* airspeed index: + / 0: first airspeed sensor valid + / 1: second airspeed sensor valid + / -1: airspeed sensor(s) invalid, but groundspeed-windspeed estimate valid + / -2: airspeed invalid (sensors and groundspeed-windspeed estimate invalid) + */ + bool find_new_valid_index = false; + + /* find new valid index if airspeed currently invalid (but we have sensors) */ + if ((_number_of_airspeed_sensors > 0 && _prev_airspeed_index < 0) || + (_prev_airspeed_index >= 0 && !_airspeed_validator[_prev_airspeed_index].get_airspeed_valid()) || + _prev_airspeed_index == -2) { + + find_new_valid_index = true; + } + + if (find_new_valid_index) { + _valid_airspeed_index = -1; + + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + if (_airspeed_validator[i].get_airspeed_valid()) { + _valid_airspeed_index = i; + break; + } + } + } + + if (_valid_airspeed_index < 0 && !_vehicle_local_position_valid) { + _valid_airspeed_index = -2; + } + + /* publish critical message (and log) in index has changed */ + if (_valid_airspeed_index != _prev_airspeed_index) { + mavlink_log_critical(&_mavlink_log_pub, "Airspeed: switched from sensor %i to %i", _prev_airspeed_index, + _valid_airspeed_index); + } + + _prev_airspeed_index = _valid_airspeed_index; + + /* fill out airspeed_validated message for publishing it */ + airspeed_validated_s airspeed_validated = {}; + airspeed_validated.timestamp = hrt_absolute_time(); + airspeed_validated.true_ground_minus_wind_m_s = NAN; + airspeed_validated.indicated_ground_minus_wind_m_s = NAN; + airspeed_validated.indicated_airspeed_m_s = NAN; + airspeed_validated.equivalent_airspeed_m_s = NAN; + airspeed_validated.true_airspeed_m_s = NAN; + + airspeed_validated.selected_airspeed_index = _valid_airspeed_index; + + switch (_valid_airspeed_index) { + case -2: + break; + + case -1: + airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; + airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS; + break; + + default: + airspeed_validated.indicated_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_IAS(); + airspeed_validated.equivalent_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_EAS(); + airspeed_validated.true_airspeed_m_s = _airspeed_validator[_valid_airspeed_index].get_TAS(); + airspeed_validated.true_ground_minus_wind_m_s = _ground_minus_wind_TAS; + airspeed_validated.indicated_ground_minus_wind_m_s = _ground_minus_wind_EAS; + break; + } + + /* publish airspeed validated topic */ + _airspeed_validated_pub.publish(airspeed_validated); + + /* publish sideslip-only-fusion wind topic */ + _wind_est_pub[0].publish(_wind_estimate_sideslip); + + /* publish the wind estimator states from all airspeed validators */ + for (int i = 0; i < _number_of_airspeed_sensors; i++) { + wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(hrt_absolute_time()); + _wind_est_pub[i + 1].publish(wind_est); + } + +} + +int AirspeedModule::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + int ret = AirspeedModule::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int AirspeedModule::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module provides a single airspeed_validated topic, containing an indicated (IAS), +equivalend (EAS), true airspeed (TAS) and the information if the estimation currently +is invalid and if based sensor readings or on groundspeed minus windspeed. +Supporting the input of multiple "raw" airspeed inputs, this module automatically switches +to a valid sensor in case of failure detection. For failure detection as well as for +the estimation of a scale factor from IAS to EAS, it runs several wind estimators +and also publishes those. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("airspeed_estimator", "estimator"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int AirspeedModule::print_status() +{ + perf_print_counter(_perf_elapsed); + perf_print_counter(_perf_interval); + + int instance = 0; + uORB::SubscriptionData est{ORB_ID(airspeed_validated), (uint8_t)instance}; + est.update(); + PX4_INFO("Number of airspeed sensors: %i", _number_of_airspeed_sensors); + print_message(est.get()); + + return 0; +} + +extern "C" __EXPORT int airspeed_selector_main(int argc, char *argv[]); + +int +airspeed_selector_main(int argc, char *argv[]) +{ + return AirspeedModule::main(argc, argv); +} diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c new file mode 100644 index 000000000000..69572318ebde --- /dev/null +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -0,0 +1,93 @@ + +/** + * Airspeed Selector: Wind estimator wind process noise + * + * Wind process noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 1 + * @unit m/s/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_W_P_NOISE, 0.1f); + +/** + * Airspeed Selector: Wind estimator true airspeed scale process noise + * + * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 0.1 + * @unit 1/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_SC_P_NOISE, 0.0001); + +/** + * Airspeed Selector: Wind estimator true airspeed measurement noise + * + * True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 4 + * @unit m/s + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_TAS_NOISE, 1.4); + +/** + * Airspeed Selector: Wind estimator sideslip measurement noise + * + * Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector. + * + * @min 0 + * @max 1 + * @unit rad + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_BETA_NOISE, 0.3); + +/** + * Airspeed Selector: Gate size for true airspeed fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @min 1 + * @max 5 + * @unit SD + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_TAS_GATE, 3); + +/** + * Airspeed Selector: Gate size for true sideslip fusion + * + * Sets the number of standard deviations used by the innovation consistency test. + * + * @min 1 + * @max 5 + * @unit SD + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_BETA_GATE, 1); + +/** + * Automatic airspeed scale estimation on + * + * Turns the automatic airspeed scale (scale from IAS to CAS/EAS) on or off. It is recommended level (keeping altitude) while performing the estimation. Set to 1 to start estimation (best when already flying). Set to 0 to end scale estimation. The estimated scale is then saved in the ARSP_ARSP_SCALE parameter. + * + * @boolean + * @group Airspeed Validator + */ +PARAM_DEFINE_INT32(ARSP_SCALE_EST, 0); + +/** + * Airspeed scale (scale from IAS to CAS/EAS) + * + * Scale can either be entered manually, or estimated in-flight by setting ARSP_SCALE_EST to 1. + * + * @min 0.5 + * @max 1.5 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ARSP_ARSP_SCALE, 1.0f); diff --git a/src/modules/attitude_estimator_q/CMakeLists.txt b/src/modules/attitude_estimator_q/CMakeLists.txt index 09c8fc7e2c01..6563648fc50e 100644 --- a/src/modules/attitude_estimator_q/CMakeLists.txt +++ b/src/modules/attitude_estimator_q/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MODULE modules__attitude_estimator_q MAIN attitude_estimator_q COMPILE_FLAGS - STACK_MAIN 1200 STACK_MAX 1600 SRCS attitude_estimator_q_main.cpp diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a8f5165a8d1e..39b86441645a 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -425,9 +425,6 @@ void AttitudeEstimatorQ::task_main() if (update(dt)) { vehicle_attitude_s att = {}; att.timestamp = sensors.timestamp; - att.rollspeed = _rates(0); - att.pitchspeed = _rates(1); - att.yawspeed = _rates(2); _q.copyTo(att.q); /* the instance count is not used here */ diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c46780167e85..d90db8681741 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -36,10 +36,7 @@ add_subdirectory(failure_detector) px4_add_module( MODULE modules__commander MAIN commander - STACK_MAIN 4096 - STACK_MAX 2450 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS accelerometer_calibration.cpp airspeed_calibration.cpp diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0349882b1a1d..58e6d638c7b0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -93,9 +93,9 @@ #include #include #include -#include #include #include +#include typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ @@ -198,14 +198,17 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy); +bool shutdown_if_allowed(); + +transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); -static void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); +static void answer_command(const vehicle_command_s &cmd, unsigned result, + uORB::PublicationQueued &command_ack_pub); static int power_button_state_notification_cb(board_power_button_state_notification_e request) { @@ -249,7 +252,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN) { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); vcmd.param1 = param1; vcmd.param2 = param2; @@ -262,9 +266,9 @@ static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 vcmd.target_system = status.system_id; vcmd.target_component = status.component_id; - orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; - return (h != nullptr); + return vcmd_pub.publish(vcmd); } int commander_main(int argc, char *argv[]) @@ -379,7 +383,13 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) { + bool force_arming = false; + + if (argc > 2 && !strcmp(argv[2], "-f")) { + force_arming = true; + } + + if (TRANSITION_CHANGED != arm_disarm(true, !force_arming, &mavlink_log_pub, "command line")) { PX4_ERR("arming failed"); } @@ -387,7 +397,7 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "disarm")) { - if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) { + if (TRANSITION_DENIED == arm_disarm(false, true, &mavlink_log_pub, "command line")) { PX4_ERR("rejected disarm"); } @@ -401,7 +411,7 @@ int commander_main(int argc, char *argv[]) /* see if we got a home position */ if (status_flags.condition_local_position_valid) { - if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { + if (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "command line")) { ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); } else { @@ -508,11 +518,34 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason && *reason > 0) { + if (reason) { PX4_INFO("%s", reason); } - PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}\n"); + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The commander module contains the state machine for mode switching and failsafe behavior. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("commander", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); + PRINT_MODULE_USAGE_ARG("mag|accel|gyro|level|esc|airspeed", "Calibration type", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); + PRINT_MODULE_USAGE_COMMAND("arm"); + PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); + PRINT_MODULE_USAGE_COMMAND("disarm"); + PRINT_MODULE_USAGE_COMMAND("takeoff"); + PRINT_MODULE_USAGE_COMMAND("land"); + PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); + PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|rattitude|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", + "Flight mode", false); + PRINT_MODULE_USAGE_COMMAND("lockdown"); + PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } void print_status() @@ -520,7 +553,15 @@ void print_status() PX4_INFO("arming: %s", arming_state_names[status.arming_state]); } -transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy) +bool shutdown_if_allowed() +{ + return TRANSITION_DENIED != arming_state_transition(&status, safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, + &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, arm_requirements, + hrt_elapsed_time(&commander_boot_timestamp)); +} + +transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, + const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -530,7 +571,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed, - true /* fRunPreArmChecks */, + run_preflight_checks, mavlink_log_pub_local, &status_flags, arm_requirements, @@ -550,7 +591,7 @@ Commander::Commander() : ModuleParams(nullptr), _failure_detector(this) { - _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); _auto_disarm_killed.set_hysteresis_time_from(false, 5_s); // We want to accept RC inputs as default @@ -577,7 +618,7 @@ Commander::~Commander() bool Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local, - orb_advert_t *command_ack_pub, bool *changed) + uORB::PublicationQueued &command_ack_pub, bool *changed) { /* only handle commands that are meant to be handled by this system and component */ if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) @@ -807,7 +848,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } } - transition_result_t arming_res = arm_disarm(cmd_arms, &mavlink_log_pub, "Arm/Disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, true, &mavlink_log_pub, "Arm/Disarm component command"); if (arming_res == TRANSITION_DENIED) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -1011,7 +1052,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ // switch to AUTO_MISSION and ARM if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state)) - && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "Mission start command"))) { + && (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "Mission start command"))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1078,13 +1119,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(cmd, cmd_result, *command_ack_pub); + answer_command(cmd, cmd_result, command_ack_pub); } return true; @@ -1188,6 +1229,7 @@ Commander::run() param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN"); param_t _param_rc_override = param_find("COM_RC_OVERRIDE"); param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ"); + param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS"); param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID"); param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT"); @@ -1201,9 +1243,6 @@ Commander::run() param_t _param_airmode = param_find("MC_AIRMODE"); param_t _param_rc_map_arm_switch = param_find("RC_MAP_ARM_SW"); - /* failsafe response to loss of navigation accuracy */ - param_t _param_posctl_nav_loss_act = param_find("COM_POSCTL_NAVL"); - status_flags.avoidance_system_required = _param_com_obs_avoid.get(); /* pthread for slow low prio thread */ @@ -1233,13 +1272,14 @@ Commander::run() PX4_ERR("Failed to register power notification callback"); } + get_circuit_breaker_params(); /* armed topic */ hrt_abstime last_disarmed_timestamp = 0; /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_init(); @@ -1265,6 +1305,7 @@ Commander::run() uORB::Subscription subsys_sub{ORB_ID(subsystem_info)}; uORB::Subscription system_power_sub{ORB_ID(system_power)}; uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)}; + uORB::Subscription esc_status_sub{ORB_ID(esc_status)}; geofence_result_s geofence_result {}; @@ -1319,6 +1360,10 @@ Commander::run() param_get(_param_arm_mission_required, &arm_mission_required_param); arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + int32_t arm_escs_checks_required_param = 0; + param_get(_param_escs_checks_required, &arm_escs_checks_required_param); + arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + status.rc_input_mode = rc_in_off; // user adjustable duration required to assert arm/disarm via throttle/rudder stick @@ -1326,7 +1371,6 @@ Commander::run() param_get(_param_rc_arm_hyst, &rc_arm_hyst); rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; - int32_t posctl_nav_loss_act = 0; int32_t geofence_action = 0; int32_t flight_uuid = 0; int32_t airmode = 0; @@ -1448,6 +1492,9 @@ Commander::run() arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; param_get(_param_arm_mission_required, &arm_mission_required_param); arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + param_get(_param_escs_checks_required, &arm_escs_checks_required_param); + arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + /* flight mode slots */ param_get(_param_fmode_1, &_flight_mode_slots[0]); @@ -1457,9 +1504,6 @@ Commander::run() param_get(_param_fmode_5, &_flight_mode_slots[4]); param_get(_param_fmode_6, &_flight_mode_slots[5]); - /* failsafe response to loss of navigation accuracy */ - param_get(_param_posctl_nav_loss_act, &posctl_nav_loss_act); - param_get(_param_takeoff_finished_action, &takeoff_complete_act); /* check for unsafe Airmode settings: yaw airmode requires the use of an arming switch */ @@ -1510,7 +1554,7 @@ Commander::run() } /* if the USB hardware connection went away, reboot */ - if (status_flags.usb_connected && !system_power.usb_connected) { + if (status_flags.usb_connected && !system_power.usb_connected && shutdown_if_allowed()) { /* * apparently the USB cable went away but we are still powered, * so lets reset to a classic non-usb state. @@ -1600,6 +1644,14 @@ Commander::run() } } + if (esc_status_sub.updated()) { + /* ESCs status changed */ + esc_status_s esc_status = {}; + + esc_status_sub.copy(&esc_status); + esc_status_check(esc_status); + } + estimator_check(&status_changed); airspeed_use_check(); @@ -1641,30 +1693,28 @@ Commander::run() // Auto disarm when landed or kill switch engaged if (armed.armed) { - // Check for auto-disarm on landing - if (_param_com_disarm_land.get() > 0) { + // Check for auto-disarm on landing or pre-flight + if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { - if (!have_taken_off_since_arming) { - // pilot has ten seconds time to take off - _auto_disarm_landed.set_hysteresis_time_from(false, 10_s); - - } else { + if (_param_com_disarm_land.get() > 0 && have_taken_off_since_arming) { _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); - } + _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); - _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); + } else if (_param_com_disarm_preflight.get() > 0 && !have_taken_off_since_arming) { + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); + } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, &mavlink_log_pub, "Auto disarm on land"); + arm_disarm(false, true, &mavlink_log_pub, "Auto disarm initiated"); } } - // Auto disarm after 5 seconds if kill switch is engaged _auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time()); if (_auto_disarm_killed.get_state()) { - arm_disarm(false, &mavlink_log_pub, "Kill-switch still engaged, disarming"); + arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged, disarming"); } } else { @@ -2152,7 +2202,7 @@ Commander::run() cmd_sub.copy(&cmd); /* handle it */ - if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) { + if (handle_command(&status, cmd, &armed, command_ack_pub, &status_changed)) { status_changed = true; } } @@ -2258,9 +2308,9 @@ Commander::run() status_flags, land_detector.landed, (link_loss_actions_t)_param_nav_rcl_act.get(), - _param_com_obl_act.get(), - _param_com_obl_rc_act.get(), - posctl_nav_loss_act); + (offboard_loss_actions_t)_param_com_obl_act.get(), + (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(), + (position_nav_loss_actions_t)_param_com_posctl_navl.get()); if (status.failsafe != failsafe_old) { status_changed = true; @@ -2283,18 +2333,34 @@ Commander::run() status.timestamp = hrt_absolute_time(); _status_pub.publish(status); - /* set prearmed state if safety is off, or safety is not present and 5 seconds passed */ - if (safety.safety_switch_available) { - - /* safety is off, go into prearmed */ - armed.prearmed = safety.safety_off; + switch ((PrearmedMode)_param_com_prearm_mode.get()) { + case PrearmedMode::DISABLED: + /* skip prearmed state */ + armed.prearmed = false; + break; - } else { + case PrearmedMode::ALWAYS: /* safety is not present, go into prearmed - * (all output drivers should be started / unlocked last in the boot process - * when the rest of the system is fully initialized) - */ + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ armed.prearmed = (hrt_elapsed_time(&commander_boot_timestamp) > 5_s); + break; + + case PrearmedMode::SAFETY_BUTTON: + if (safety.safety_switch_available) { + /* safety switch is present, go into prearmed if safety is off */ + armed.prearmed = safety.safety_off; + + } else { + /* safety switch is not present, do not go into prearmed */ + armed.prearmed = false; + } + break; + + default: + armed.prearmed = false; + break; } armed.timestamp = hrt_absolute_time(); @@ -2435,7 +2501,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu { static hrt_abstime overload_start = 0; - bool overload = (cpuload_local->load > 0.80f) || (cpuload_local->ram_usage > 0.98f); + bool overload = (cpuload_local->load > 0.95f) || (cpuload_local->ram_usage > 0.98f); if (overload_start == 0 && overload) { overload_start = hrt_absolute_time(); @@ -3260,7 +3326,8 @@ print_reject_arm(const char *msg) } } -void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub) +void answer_command(const vehicle_command_s &cmd, unsigned result, + uORB::PublicationQueued &command_ack_pub) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -3288,20 +3355,16 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t } /* publish ACK */ - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); - } else { - command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + command_ack_pub.publish(command_ack); } void *commander_low_prio_loop(void *arg) @@ -3313,7 +3376,7 @@ void *commander_low_prio_loop(void *arg) int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; /* wakeup source(s) */ px4_pollfd_struct_t fds[1]; @@ -3603,7 +3666,7 @@ Commander *Commander::instantiate(int argc, char *argv[]) Commander *instance = new Commander(); if (instance) { - if (argc >= 2 && !strcmp(argv[1], "--hil")) { + if (argc >= 2 && !strcmp(argv[1], "-h")) { instance->enable_hil(); } } @@ -3860,9 +3923,9 @@ void Commander::battery_status_check() } // Handle shutdown request from emergency battery action - if (!armed.armed && (battery.warning != _battery_warning)) { + if (battery.warning != _battery_warning) { - if (battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + if ((battery.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) { mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down"); px4_usleep(200000); @@ -3894,8 +3957,8 @@ void Commander::airspeed_use_check() _airspeed_sub.update(); const airspeed_s &airspeed = _airspeed_sub.get(); - _sensor_bias_sub.update(); - const sensor_bias_s &sensors = _sensor_bias_sub.get(); + vehicle_acceleration_s accel{}; + _vehicle_acceleration_sub.copy(&accel); bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; @@ -3981,7 +4044,7 @@ void Commander::airspeed_use_check() float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f); max_lift_ratio *= max_lift_ratio; - _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(sensors.accel_z) / CONSTANTS_ONE_G) / max_lift_ratio; + _load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel.xyz[2]) / CONSTANTS_ONE_G) / max_lift_ratio; _load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f); load_factor_ratio_fail = (_load_factor_ratio > 1.1f); status.load_factor_ratio = _load_factor_ratio; @@ -4283,7 +4346,7 @@ Commander::offboard_control_update(bool &status_changed) if (commander_state_s::MAIN_STATE_OFFBOARD) { if (offboard_control_mode.timestamp == 0) { - _offboard_control_mode_sub.forceInit(); + _offboard_control_mode_sub.subscribe(); force_update = true; } } @@ -4340,3 +4403,39 @@ Commander::offboard_control_update(bool &status_changed) } } + +void Commander::esc_status_check(const esc_status_s &esc_status) +{ + char esc_fail_msg[50]; + esc_fail_msg[0] = '\0'; + + int online_bitmask = (1 << esc_status.esc_count) - 1; + + // Check if ALL the ESCs are online + if (online_bitmask == esc_status.esc_online_flags) { + status_flags.condition_escs_error = false; + _last_esc_online_flags = esc_status.esc_online_flags; + + } else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0) { + + // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver + + status_flags.condition_escs_error = true; + + } else if (esc_status.esc_online_flags < _last_esc_online_flags) { + + // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot + + for (int index = 0; index < esc_status.esc_count; index++) { + if ((esc_status.esc_online_flags & (1 << index)) == 0) { + snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); + esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; + } + } + + mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg); + + _last_esc_online_flags = esc_status.esc_online_flags; + status_flags.condition_escs_error = true; + } +} diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e29f54e44ada..5fad5ce8084b 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -45,8 +45,10 @@ // publications #include +#include #include #include +#include #include #include #include @@ -58,11 +60,12 @@ #include #include #include -#include #include +#include #include #include #include +#include using math::constrain; @@ -113,6 +116,7 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_pos_fs_eph, (ParamFloat) _param_com_pos_fs_epv, (ParamFloat) _param_com_vel_fs_evh, + (ParamInt) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */ (ParamInt) _param_com_pos_fs_delay, (ParamInt) _param_com_pos_fs_prob, @@ -120,6 +124,7 @@ class Commander : public ModuleBase, public ModuleParams (ParamInt) _param_com_low_bat_act, (ParamFloat) _param_com_disarm_land, + (ParamFloat) _param_com_disarm_preflight, (ParamInt) _param_com_obs_avoid, (ParamInt) _param_com_oa_boot_t, @@ -135,10 +140,18 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_of_loss_t, (ParamInt) _param_com_obl_act, - (ParamInt) _param_com_obl_rc_act + (ParamInt) _param_com_obl_rc_act, + + (ParamInt) _param_com_prearm_mode ) + enum class PrearmedMode { + DISABLED = 0, + SAFETY_BUTTON = 1, + ALWAYS = 2 + }; + const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */ const int64_t POSVEL_PROBATION_MAX = 100_s; /**< maximum probation duration (usec) */ @@ -180,7 +193,7 @@ class Commander : public ModuleBase, public ModuleParams bool _flight_termination_triggered{false}; bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed, - orb_advert_t *command_ack_pub, bool *changed); + uORB::PublicationQueued &command_ack_pub, bool *changed); bool set_home_position(); bool set_home_position_alt_only(); @@ -215,6 +228,8 @@ class Commander : public ModuleBase, public ModuleParams void battery_status_check(); + void esc_status_check(const esc_status_s &esc_status); + /** * Checks the status of all available data links and handles switching between different system telemetry states. */ @@ -238,6 +253,8 @@ class Commander : public ModuleBase, public ModuleParams hrt_abstime _high_latency_datalink_heartbeat{0}; hrt_abstime _high_latency_datalink_lost{0}; + int _last_esc_online_flags{-1}; + uORB::Subscription _battery_sub{ORB_ID(battery_status)}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; float _battery_current{0.0f}; @@ -248,22 +265,24 @@ class Commander : public ModuleBase, public ModuleParams bool _print_avoidance_msg_once{false}; // Subscriptions + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; uORB::SubscriptionData _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; - uORB::SubscriptionData _sensor_bias_sub{ORB_ID(sensor_bias)}; uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; // Publications uORB::Publication _control_mode_pub{ORB_ID(vehicle_control_mode)}; - uORB::PublicationData _home_pub{ORB_ID(home_position)}; uORB::Publication _status_pub{ORB_ID(vehicle_status)}; uORB::Publication _armed_pub{ORB_ID(actuator_armed)}; uORB::Publication _commander_state_pub{ORB_ID(commander_state)}; uORB::Publication _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)}; + uORB::PublicationData _home_pub{ORB_ID(home_position)}; + }; #endif /* COMMANDER_HPP_ */ diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index b8ad44a1a462..e310bb689488 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -45,6 +45,7 @@ #include "rc_check.h" #include +#include #include #include @@ -203,6 +204,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s // return false if the magnetomer measurements are inconsistent static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_status) { + bool pass = false; // flag for result of checks + // get the sensor preflight data uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; sensors_sub.update(); @@ -210,25 +213,26 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s if (sensors.timestamp == 0) { // can happen if not advertised (yet) - return true; + pass = true; } // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. // If a single sensor is fitted, the value being checked will be zero so this check will always pass. - float test_limit; - param_get(param_find("COM_ARM_MAG"), &test_limit); + int32_t angle_difference_limit_deg; + param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg); - if (sensors.mag_inconsistency_ga > test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); - } + pass = pass || angle_difference_limit_deg < 0; // disabled, pass check + pass = pass || sensors.mag_inconsistency_angle < math::radians(angle_difference_limit_deg); - return false; + if (!pass && report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent", + static_cast(math::degrees(sensors.mag_inconsistency_angle))); + mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate"); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); } - return true; + return pass; } static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance, diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/arm_auth.cpp index 891f9c703809..5be6cc73b485 100644 --- a/src/modules/commander/arm_auth.cpp +++ b/src/modules/commander/arm_auth.cpp @@ -39,10 +39,10 @@ #include #include +#include #include #include -static orb_advert_t handle_vehicle_command_pub; static orb_advert_t *mavlink_log_pub; static int command_ack_sub = -1; @@ -57,13 +57,16 @@ static enum { ARM_AUTH_MISSION_APPROVED } state = ARM_AUTH_IDLE; -struct packed_struct { - uint8_t authorizer_system_id; - union { - uint16_t auth_method_arm_timeout_msec; - uint16_t auth_method_two_arm_timeout_msec; - } auth_method_param; - uint8_t authentication_method; +union { + struct { + uint8_t authorizer_system_id; + union { + uint16_t auth_method_arm_timeout_msec; + uint16_t auth_method_two_arm_timeout_msec; + } auth_method_param; + uint8_t authentication_method; + } __attribute__((packed)) struct_value; + int32_t param_value; } arm_parameters; static uint8_t *system_id; @@ -78,17 +81,13 @@ static uint8_t (*arm_check_method[ARM_AUTH_METHOD_LAST])() = { static void arm_auth_request_msg_send() { - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); vcmd.command = vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST; - vcmd.target_system = arm_parameters.authorizer_system_id; + vcmd.target_system = arm_parameters.struct_value.authorizer_system_id; - if (handle_vehicle_command_pub == nullptr) { - handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd); - } + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + vcmd_pub.publish(vcmd); } static uint8_t _auth_method_arm_req_check() @@ -109,7 +108,7 @@ static uint8_t _auth_method_arm_req_check() arm_auth_request_msg_send(); hrt_abstime now = hrt_absolute_time(); - auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000); + auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; while (now < auth_timeout) { @@ -161,7 +160,7 @@ static uint8_t _auth_method_two_arm_check() arm_auth_request_msg_send(); hrt_abstime now = hrt_absolute_time(); - auth_timeout = now + (arm_parameters.auth_method_param.auth_method_arm_timeout_msec * 1000); + auth_timeout = now + (arm_parameters.struct_value.auth_method_param.auth_method_arm_timeout_msec * 1000); state = ARM_AUTH_WAITING_AUTH; mavlink_log_critical(mavlink_log_pub, "Arm auth: Requesting authorization..."); @@ -171,8 +170,8 @@ static uint8_t _auth_method_two_arm_check() uint8_t arm_auth_check() { - if (arm_parameters.authentication_method < ARM_AUTH_METHOD_LAST) { - return arm_check_method[arm_parameters.authentication_method](); + if (arm_parameters.struct_value.authentication_method < ARM_AUTH_METHOD_LAST) { + return arm_check_method[arm_parameters.struct_value.authentication_method](); } return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; @@ -181,7 +180,7 @@ uint8_t arm_auth_check() void arm_auth_update(hrt_abstime now, bool param_update) { if (param_update) { - param_get(param_arm_parameters, (int32_t *)&arm_parameters); + param_get(param_arm_parameters, &arm_parameters.param_value); } switch (state) { @@ -284,5 +283,5 @@ void arm_auth_init(orb_advert_t *mav_log_pub, uint8_t *sys_id) enum arm_auth_methods arm_auth_method_get() { - return (enum arm_auth_methods) arm_parameters.authentication_method; + return (enum arm_auth_methods) arm_parameters.struct_value.authentication_method; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index a0107297e771..d8e1554c769a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -290,7 +290,7 @@ int led_init() led_control.mode = led_control_s::MODE_OFF; led_control.priority = 0; led_control.timestamp = hrt_absolute_time(); - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); + led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, led_control_s::ORB_QUEUE_LENGTH); /* first open normal LEDs */ DevMgr::getHandle(LED0_DEVICE_PATH, h_leds); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index ebe7c7446663..4f884f4edcf5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -262,19 +262,30 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000); * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be * automatically disarmed in case a landing situation has been detected during this period. * - * The vehicle will also auto-disarm right after arming if it has not even flown, however the time - * will always be 10 seconds such that the pilot has enough time to take off. - * - * A negative value means that automatic disarming triggered by landing detection is disabled. + * A zero or negative value means that automatic disarming triggered by landing detection is disabled. * * @group Commander - * @min -1 - * @max 20 * @unit s * @decimal 2 */ + PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f); +/** + * Time-out for auto disarm if too slow to takeoff + * + * A non-zero, positive value specifies the time after arming, in seconds, within which the + * vehicle must take off (after which it will automatically disarm). + * + * A zero or negative value means that automatic disarming triggered by a pre-takeoff timeout is disabled. + * + * @group Commander + * @unit s + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); + + /** * Allow arming without GPS * @@ -333,9 +344,12 @@ PARAM_DEFINE_FLOAT(COM_OF_LOSS_T, 0.0f); * The offboard loss failsafe will only be entered after a timeout, * set by COM_OF_LOSS_T in seconds. * - * @value 0 Land mode - * @value 1 Hold mode - * @value 2 Return mode + * @value -1 Disabled + * @value 0 Land mode + * @value 1 Hold mode + * @value 2 Return mode + * @value 3 Terminate + * @value 4 Lockdown * * @group Commander */ @@ -347,12 +361,15 @@ PARAM_DEFINE_INT32(COM_OBL_ACT, 0); * The offboard loss failsafe will only be entered after a timeout, * set by COM_OF_LOSS_T in seconds. * - * @value 0 Position mode - * @value 1 Altitude mode - * @value 2 Manual - * @value 3 Return mode - * @value 4 Land mode - * @value 5 Hold mode + * @value -1 Disabled + * @value 0 Position mode + * @value 1 Altitude mode + * @value 2 Manual + * @value 3 Return mode + * @value 4 Land mode + * @value 5 Hold mode + * @value 6 Terminate + * @value 7 Lockdown * @group Commander */ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); @@ -600,15 +617,14 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f); /** * Maximum magnetic field inconsistency between units that will allow arming + * Set -1 to disable the check. * * @group Commander - * @unit Gauss - * @min 0.05 - * @max 0.5 - * @decimal 2 - * @increment 0.05 + * @unit deg + * @min 3 + * @max 180 */ -PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f); +PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 30); /** * Enable RC stick override of auto modes @@ -639,8 +655,8 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control. * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. * - * @value 0 Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. - * @value 1 Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. + * @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + * @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. * * @group Commander */ @@ -927,3 +943,27 @@ PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0); * @group Commander */ PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0); + +/** + * Require all the ESCs to be detected to arm. + * + * This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param. + * + * @group Commander + * @boolean + */ +PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1); + +/** + * Condition to enter prearmed mode + * + * Condition to enter the prearmed state, an intermediate state between disarmed and armed + * in which non-throttling actuators are active. + * + * @value 0 Disabled + * @value 1 Safety button + * @value 2 Always + * + * @group Commander + */ +PARAM_DEFINE_INT32(COM_PREARM_MODE, 1); diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 2231c3092322..6d663fcb7f8d 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -128,8 +128,8 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "transition: init to reboot", { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_REBOOT, - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + vehicle_status_s::ARMING_STATE_SHUTDOWN, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { @@ -149,8 +149,8 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "transition: standby to reboot", { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_REBOOT, - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + vehicle_status_s::ARMING_STATE_SHUTDOWN, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { @@ -163,8 +163,8 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "transition: standby error to reboot", { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_REBOOT, - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + vehicle_status_s::ARMING_STATE_SHUTDOWN, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { @@ -177,8 +177,8 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "transition: in air restore to reboot", { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_REBOOT, - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + vehicle_status_s::ARMING_STATE_SHUTDOWN, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, // hil on tests, standby error to standby not normally allowed @@ -194,14 +194,14 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "transition: standby to armed, no safety switch", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { "transition: standby to armed, safety switch off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, @@ -227,7 +227,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "no transition: armed to reboot", { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_REBOOT, + vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, @@ -247,9 +247,9 @@ bool StateMachineHelperTest::armingStateTransitionTest() { "no transition: reboot to armed", - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { @@ -269,7 +269,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() // Safety switch arming tests { - "no transition: init to standby, safety switch on", + "no transition: init to armed, safety switch on", { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED @@ -301,7 +301,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() // Attempt transition transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed, - false /* no pre-arm checks */, + true /* enable pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, (check_gps ? ARM_REQ_GPS_BIT : 0), diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index be7cffa469b1..1f4cbf7692af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -66,13 +66,13 @@ static constexpr const char reason_no_datalink[] = "no datalink"; // code for those checks. static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_REBOOT */ true, true, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get its textual representation @@ -81,7 +81,7 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { "STANDBY", "ARMED", "STANDBY_ERROR", - "REBOOT", + "SHUTDOWN", "IN_AIR_RESTORE", }; @@ -93,6 +93,18 @@ void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, const link_loss_actions_t link_loss_act); +void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_actions_t offboard_loss_act); + +void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_rc_actions_t offboard_loss_rc_act); + +void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe, + const offboard_loss_actions_t offboard_loss_act, + const offboard_loss_rc_actions_t offboard_loss_rc_act); + transition_result_t arming_state_transition(vehicle_status_s *status, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s *armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, vehicle_status_flags_s *status_flags, const uint8_t arm_requirements, @@ -119,7 +131,6 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe * Get sensing state if necessary */ bool preflight_check_ret = true; - bool prearm_check_ret = true; const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); @@ -162,15 +173,16 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON - if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && - !hil_enabled) { + if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE && !hil_enabled) { - if (preflight_check_ret) { + bool prearm_check_ret = true; + + if (fRunPreArmChecks && preflight_check_ret) { // only bother running prearm if preflight was successful prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, safety, arm_requirements); } - if (!(preflight_check_ret && prearm_check_ret)) { + if (!preflight_check_ret || !prearm_check_ret) { // the prearm and preflight checks already print the rejection reason feedback_provided = true; valid_transition = false; @@ -392,8 +404,9 @@ void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t * bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state, orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, - const link_loss_actions_t rc_loss_act, const int offb_loss_act, const int offb_loss_rc_act, - const int posctl_nav_loss_act) + const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, + const offboard_loss_rc_actions_t offb_loss_rc_act, + const position_nav_loss_actions_t posctl_nav_loss_act) { navigation_state_t nav_state_old = status->nav_state; @@ -408,6 +421,7 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ // Safe to do reset flags here, as if loss state persists flags will be restored in the code below reset_link_loss_globals(armed, old_failsafe, rc_loss_act); reset_link_loss_globals(armed, old_failsafe, data_link_loss_act); + reset_offboard_loss_globals(armed, old_failsafe, offb_loss_act, offb_loss_rc_act); /* evaluate main state to decide in normal (non-failsafe) mode */ switch (internal_state->main_state) { @@ -468,7 +482,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ * For fixedwing, a global position is needed. */ } else if (is_armed - && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), + && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, + !(posctl_nav_loss_act == position_nav_loss_actions_t::LAND_TERMINATE), status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) { // nothing to do - everything done in check_invalid_pos_nav_state @@ -593,8 +608,17 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ // failsafe: on engine failure status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + // Orbit can only be started via vehicle_command (mavlink). Recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) { - // failsafe: necessary position estimate lost (nothing to do - everything done in check_invalid_pos_nav_state) + // failsafe: necessary position estimate lost; switching is done in check_invalid_pos_nav_state + + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } else if (status->data_link_lost && data_link_loss_act_configured && !landed && is_armed) { // failsafe: just datalink is lost and we're in air set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, @@ -602,6 +626,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } else if (rc_lost && !data_link_loss_act_configured && is_armed) { // failsafe: RC is lost, datalink loss is not set up and rc loss is not disabled enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); @@ -609,6 +637,10 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); + // Orbit can only be started via vehicle_command (mavlink). Consequently, recovery from failsafe into orbit + // is not possible and therefore the internal_state needs to be adjusted. + internal_state->main_state = commander_state_s::MAIN_STATE_POSCTL; + } else { // no failsafe, RC is not mandatory for orbit status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT; @@ -667,98 +699,16 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ case commander_state_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ - if (status_flags.offboard_control_signal_lost && !status->rc_signal_lost) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); - - if (status_flags.offboard_control_loss_timeout && offb_loss_rc_act < 6 && offb_loss_rc_act >= 0) { - if (offb_loss_rc_act == 3 && status_flags.condition_global_position_valid - && status_flags.condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - - } else if (offb_loss_rc_act == 0 && status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - - } else if (offb_loss_rc_act == 1 && status_flags.condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - - } else if (offb_loss_rc_act == 2) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; - - } else if (offb_loss_rc_act == 4 && status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - - } else if (offb_loss_rc_act == 5 && status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - - } else if (status_flags.condition_local_altitude_valid) { - //TODO: Add case for rover - if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - - } else { - if (status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; - - } else if (status_flags.condition_local_altitude_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } - } - - } else if (status_flags.offboard_control_signal_lost && status->rc_signal_lost) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard); - - if (status_flags.offboard_control_loss_timeout && offb_loss_act < 3 && offb_loss_act >= 0) { - if (offb_loss_act == 2 && status_flags.condition_global_position_valid - && status_flags.condition_home_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; - - } else if (offb_loss_act == 1 && status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - - } else if (offb_loss_act == 0 && status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; - - } else if (status_flags.condition_local_altitude_valid) { - if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } + if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_loss_timeout) { + if (status->rc_signal_lost) { + // Offboard and RC are lost + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc_and_no_offboard); + set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act); } else { - if (status_flags.condition_global_position_valid) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; - - } else if (status_flags.condition_local_altitude_valid) { - if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; - - } else { - // TODO: FW position controller doesn't run without condition_global_position_valid - status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } - - } else { - status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; - } + // Offboard is lost, RC is ok + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_offboard); + set_offboard_loss_rc_nav_state(status, armed, status_flags, offb_loss_rc_act); } } else { @@ -912,6 +862,153 @@ void reset_link_loss_globals(actuator_armed_s *armed, const bool old_failsafe, c } } +void set_offboard_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_actions_t offboard_loss_act) +{ + switch (offboard_loss_act) { + case offboard_loss_actions_t::DISABLED: + // If offboard loss failsafe is disabled then no action must be taken. + return; + + case offboard_loss_actions_t::TERMINATE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + armed->force_failsafe = true; + return; + + case offboard_loss_actions_t::LOCKDOWN: + armed->lockdown = true; + return; + + case offboard_loss_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + return; + } + + // FALLTHROUGH + case offboard_loss_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + return; + } + + // FALLTHROUGH + case offboard_loss_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + } + + } + + // If none of the above worked, try to mitigate + if (status_flags.condition_local_altitude_valid) { + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } +} + +void set_offboard_loss_rc_nav_state(vehicle_status_s *status, actuator_armed_s *armed, + const vehicle_status_flags_s &status_flags, + const offboard_loss_rc_actions_t offboard_loss_rc_act) +{ + switch (offboard_loss_rc_act) { + case offboard_loss_rc_actions_t::DISABLED: + // If offboard loss failsafe is disabled then no action must be taken. + return; + + case offboard_loss_rc_actions_t::TERMINATE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + armed->force_failsafe = true; + return; + + case offboard_loss_rc_actions_t::LOCKDOWN: + armed->lockdown = true; + return; + + case offboard_loss_rc_actions_t::MANUAL_POSITION: + if (status_flags.condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::MANUAL_ALTITUDE: + if (status_flags.condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::MANUAL_ATTITUDE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; + return; + + case offboard_loss_rc_actions_t::AUTO_RTL: + if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::AUTO_LOITER: + if (status_flags.condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; + return; + } + + // FALLTHROUGH + case offboard_loss_rc_actions_t::AUTO_LAND: + if (status_flags.condition_global_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; + return; + } + + } + + // If none of the above worked, try to mitigate + if (status_flags.condition_local_altitude_valid) { + //TODO: Add case for rover + if (status->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + + } else { + // TODO: FW position controller doesn't run without condition_global_position_valid + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } +} + +void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsafe, + const offboard_loss_actions_t offboard_loss_act, + const offboard_loss_rc_actions_t offboard_loss_rc_act) +{ + if (old_failsafe) { + if (offboard_loss_act == offboard_loss_actions_t::TERMINATE + || offboard_loss_rc_act == offboard_loss_rc_actions_t::TERMINATE) { + armed->force_failsafe = false; + + } + + if (offboard_loss_act == offboard_loss_actions_t::LOCKDOWN + || offboard_loss_rc_act == offboard_loss_rc_actions_t::LOCKDOWN) { + armed->lockdown = false; + } + } +} + bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, const uint8_t arm_requirements) { @@ -1010,6 +1107,14 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s } + if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) { + if (prearm_ok && reportFailures) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); + prearm_ok = false; + } + } + + return prearm_ok; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e6dc84c80e86..3ea6711b91a4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -64,8 +64,34 @@ enum class link_loss_actions_t { AUTO_RTL = 2, // Return mode AUTO_LAND = 3, // Land mode AUTO_RECOVER = 4, // Data Link Auto Recovery (CASA Outback Challenge rules) - TERMINATE = 5, // Turn off all controllers and set PWM outputs to failsafe value - LOCKDOWN = 6, // Kill the motors, same result as kill switch + TERMINATE = 5, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) + LOCKDOWN = 6, // Lock actuators (set actuator outputs to disarmed values) +}; + +enum class offboard_loss_actions_t { + DISABLED = -1, + AUTO_LAND = 0, // Land mode + AUTO_LOITER = 1, // Hold mode + AUTO_RTL = 2, // Return mode + TERMINATE = 3, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) + LOCKDOWN = 4, // Lock actuators (set actuator outputs to disarmed values) +}; + +enum class offboard_loss_rc_actions_t { + DISABLED = -1, // Disabled + MANUAL_POSITION = 0, // Position mode + MANUAL_ALTITUDE = 1, // Altitude mode + MANUAL_ATTITUDE = 2, // Manual + AUTO_RTL = 3, // Return mode + AUTO_LAND = 4, // Land mode + AUTO_LOITER = 5, // Hold mode + TERMINATE = 6, // Terminate flight (set actuator outputs to failsafe values, and stop controllers) + LOCKDOWN = 7, // Lock actuators (set actuator outputs to disarmed values) +}; + +enum class position_nav_loss_actions_t { + ALTITUDE_MANUAL = 0, // Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. + LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. }; typedef enum { @@ -73,6 +99,7 @@ typedef enum { ARM_REQ_MISSION_BIT = (1 << 0), ARM_REQ_ARM_AUTH_BIT = (1 << 1), ARM_REQ_GPS_BIT = (1 << 2), + ARM_REQ_ESCS_CHECK_BIT = (1 << 3) } arm_requirements_t; extern const char *const arming_state_names[]; @@ -93,8 +120,9 @@ void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t * bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_state_s *internal_state, orb_advert_t *mavlink_log_pub, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, - const link_loss_actions_t rc_loss_act, const int offb_loss_act, const int offb_loss_rc_act, - const int posctl_nav_loss_act); + const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, + const offboard_loss_rc_actions_t offb_loss_rc_act, + const position_nav_loss_actions_t posctl_nav_loss_act); /* * Checks the validty of position data aaainst the requirements of the current navigation diff --git a/src/modules/dataman/CMakeLists.txt b/src/modules/dataman/CMakeLists.txt index fc67ee0a2491..6c0a1ab69555 100644 --- a/src/modules/dataman/CMakeLists.txt +++ b/src/modules/dataman/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE modules__dataman MAIN dataman - STACK_MAIN 1200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 890e5fab59e7..9c0d1df0a900 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -34,8 +34,7 @@ px4_add_module( MODULE modules__ekf2 MAIN ekf2 COMPILE_FLAGS - STACK_MAIN 2500 - STACK_MAX 4000 + STACK_MAX 2400 SRCS ekf2_main.cpp DEPENDS diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ae99af6cdfe6..2fe3228d2625 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -283,7 +284,7 @@ class Ekf2 final : public ModuleBase, public ModuleParams uORB::Publication _sensor_bias_pub{ORB_ID(sensor_bias)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _wind_pub{ORB_ID(wind_estimate)}; + uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; @@ -1482,31 +1483,17 @@ void Ekf2::run() { // publish all corrected sensor readings and bias estimates after mag calibration is updated above - sensor_bias_s bias; + sensor_bias_s bias{}; bias.timestamp = now; // In-run bias estimates - float gyro_bias[3]; - _ekf.get_gyro_bias(gyro_bias); - bias.gyro_x_bias = gyro_bias[0]; - bias.gyro_y_bias = gyro_bias[1]; - bias.gyro_z_bias = gyro_bias[2]; - - float accel_bias[3]; - _ekf.get_accel_bias(accel_bias); - bias.accel_x_bias = accel_bias[0]; - bias.accel_y_bias = accel_bias[1]; - bias.accel_z_bias = accel_bias[2]; + _ekf.get_gyro_bias(bias.gyro_bias); + _ekf.get_accel_bias(bias.accel_bias); - bias.mag_x_bias = _last_valid_mag_cal[0]; - bias.mag_y_bias = _last_valid_mag_cal[1]; - bias.mag_z_bias = _last_valid_mag_cal[2]; - - // TODO: remove from sensor_bias? - bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0]; - bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; - bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; + bias.mag_bias[0] = _last_valid_mag_cal[0]; + bias.mag_bias[1] = _last_valid_mag_cal[1]; + bias.mag_bias[2] = _last_valid_mag_cal[2]; _sensor_bias_pub.publish(bias); } @@ -1754,13 +1741,6 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); - // In-run bias estimates - float gyro_bias[3]; - _ekf.get_gyro_bias(gyro_bias); - att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; - att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; - att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; - _att_pub.publish(att); return true; @@ -1778,19 +1758,22 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { + // Publish wind estimate only if ekf declares them valid + wind_estimate_s wind_estimate{}; float velNE_wind[2]; - _ekf.get_wind_velocity(velNE_wind); - float wind_var[2]; + _ekf.get_wind_velocity(velNE_wind); _ekf.get_wind_velocity_var(wind_var); - - // Publish wind estimate - wind_estimate_s wind_estimate; + _ekf.get_airspeed_innov(&wind_estimate.tas_innov); + _ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var); + _ekf.get_beta_innov(&wind_estimate.beta_innov); + _ekf.get_beta_innov_var(&wind_estimate.beta_innov_var); wind_estimate.timestamp = timestamp; wind_estimate.windspeed_north = velNE_wind[0]; wind_estimate.windspeed_east = velNE_wind[1]; wind_estimate.variance_north = wind_var[0]; wind_estimate.variance_east = wind_var[1]; + wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf _wind_pub.publish(wind_estimate); diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index 8246c3d9e9b0..a886cd5f4f16 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE modules__events MAIN send_event - STACK_MAIN 2200 COMPILE_FLAGS SRCS rc_loss_alarm.cpp diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 21afafb98ba0..5cd5013d9b1c 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -124,20 +124,15 @@ void SendEvent::process_commands() void SendEvent::answer_command(const vehicle_command_s &cmd, unsigned result) { /* publish ACK */ - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = cmd.command; command_ack.result = (uint8_t)result; command_ack.target_system = cmd.source_system; command_ack.target_component = cmd.source_component; - if (_command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - - } else { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); } int SendEvent::print_usage(const char *reason) diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index c1ab9f08060e..8dc4f1594630 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -36,9 +36,10 @@ #include "status_display.h" #include "rc_loss_alarm.h" -#include +#include #include #include +#include #include #include @@ -115,9 +116,6 @@ class SendEvent : public ModuleBase, public ModuleParams, public px4: /** @var _rc_loss_alarm Pointer to the RC loss alarm object. */ rc_loss::RC_Loss_Alarm *_rc_loss_alarm = nullptr; - /** @var _command_ack_pub The command ackowledgement topic. */ - orb_advert_t _command_ack_pub = nullptr; - /** @note Declare local parameters using defined parameters. */ DEFINE_PARAMETERS( /** @var _param_status_display Parameter to enable/disable the LED status display. */ diff --git a/src/modules/events/status_display.cpp b/src/modules/events/status_display.cpp index d09ccde9664a..b4fcc6b17dd7 100644 --- a/src/modules/events/status_display.cpp +++ b/src/modules/events/status_display.cpp @@ -97,12 +97,7 @@ void StatusDisplay::publish() { _led_control.timestamp = hrt_absolute_time(); - if (_led_control_pub != nullptr) { - orb_publish(ORB_ID(led_control), _led_control_pub, &_led_control); - - } else { - _led_control_pub = orb_advertise_queue(ORB_ID(led_control), &_led_control, LED_UORB_QUEUE_LENGTH); - } + _led_control_pub.publish(_led_control); } } /* namespace status */ diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index bfbf5e4d42fb..1324d13ceceb 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -42,6 +42,7 @@ #include +#include #include #include #include @@ -95,7 +96,9 @@ class StatusDisplay bool _critical_battery = false; int _old_nav_state = -1; int _old_battery_status_warning = -1; - orb_advert_t _led_control_pub = nullptr; + + uORB::PublicationQueued _led_control_pub{ORB_ID(led_control)}; + }; } /* namespace status */ diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c1b59ded7363..28963ebab915 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -45,10 +45,9 @@ using namespace time_literals; extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); FixedwingAttitudeControl::FixedwingAttitudeControl() : - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fwa_dt")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fwa_nani")), - _nonfinite_output_perf(perf_alloc(PC_COUNT, "fwa_nano")) + WorkItem(px4::wq_configurations::att_pos_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_control: cycle")), + _loop_interval_perf(perf_alloc(PC_INTERVAL, "fw_att_control: interval")) { // check if VTOL first vehicle_status_poll(); @@ -128,18 +127,23 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); - - // subscriptions - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); } FixedwingAttitudeControl::~FixedwingAttitudeControl() { - orb_unsubscribe(_att_sub); - perf_free(_loop_perf); - perf_free(_nonfinite_input_perf); - perf_free(_nonfinite_output_perf); + perf_free(_loop_interval_perf); +} + +bool +FixedwingAttitudeControl::init() +{ + if (!_att_sub.register_callback()) { + PX4_ERR("vehicle attitude callback registration failed!"); + return false; + } + + return true; } int @@ -280,6 +284,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) { + // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (_manual_sub.copy(&_manual)) { @@ -326,14 +331,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad; _rates_sp.thrust_body[0] = _manual.z; - if (_rate_sp_pub != nullptr) { - /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else { - /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } + _rate_sp_pub.publish(_rates_sp); } else { /* manual/direct control */ @@ -413,10 +411,6 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s) && !_vehicle_status.aspd_use_inhibit; - if (!airspeed_valid) { - perf_count(_nonfinite_input_perf); - } - // if no airspeed measurement is available out best guess is to use the trim airspeed float airspeed = _parameters.airspeed_trim; @@ -444,20 +438,21 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling() const float airspeed_constrained = math::constrain(airspeed, _parameters.airspeed_min, _parameters.airspeed_max); _airspeed_scaling = _parameters.airspeed_trim / airspeed_constrained; - return airspeed; } -void FixedwingAttitudeControl::run() +void FixedwingAttitudeControl::Run() { - /* wakeup source */ - px4_pollfd_struct_t fds[1]; + if (should_exit()) { + _att_sub.unregister_callback(); + exit_and_cleanup(); + return; + } - /* Setup of loop */ - fds[0].fd = _att_sub; - fds[0].events = POLLIN; + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); - while (!should_exit()) { + if (_att_sub.update(&_att)) { /* only update parameters if they changed */ bool params_updated = _params_sub.updated(); @@ -471,390 +466,347 @@ void FixedwingAttitudeControl::run() parameters_update(); } - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* only run controller if attitude changed */ + static uint64_t last_run = 0; + float deltaT = math::constrain((hrt_elapsed_time(&last_run) / 1e6f), 0.01f, 0.1f); + last_run = hrt_absolute_time(); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } + /* get current rotation matrix and euler angles from control state quaternions */ + matrix::Dcmf R = matrix::Quatf(_att.q); - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - continue; - } + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + float rollspeed = angular_velocity.xyz[0]; + float pitchspeed = angular_velocity.xyz[1]; + float yawspeed = angular_velocity.xyz[2]; - perf_begin(_loop_perf); + if (_is_tailsitter) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode + * + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + matrix::Dcmf R_adapted = R; //modified rotation matrix + + /* move z to x */ + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + + /* move x to z */ + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + /* change direction of pitch (convert to right handed system) */ + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + + /* fill in new attitude data */ + R = R_adapted; + + /* lastly, roll- and yawspeed have to be swaped */ + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; + } - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* guard against too large deltaT's */ - if (deltaT > 1.0f) { - deltaT = 0.01f; - } + const matrix::Eulerf euler_angles(R); - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - - /* get current rotation matrix and euler angles from control state quaternions */ - matrix::Dcmf R = matrix::Quatf(_att.q); - - if (_is_tailsitter) { - /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode - * - * Since the VTOL airframe is initialized as a multicopter we need to - * modify the estimated attitude for the fixed wing operation. - * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around - * the pitch axis compared to the neutral position of the vehicle in multicopter mode - * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. - * Additionally, in order to get the correct sign of the pitch, we need to multiply - * the new x axis of the rotation matrix with -1 - * - * original: modified: - * - * Rxx Ryx Rzx -Rzx Ryx Rxx - * Rxy Ryy Rzy -Rzy Ryy Rxy - * Rxz Ryz Rzz -Rzz Ryz Rxz - * */ - matrix::Dcmf R_adapted = R; //modified rotation matrix - - /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); - - /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); - - /* change direction of pitch (convert to right handed system) */ - R_adapted(0, 0) = -R_adapted(0, 0); - R_adapted(1, 0) = -R_adapted(1, 0); - R_adapted(2, 0) = -R_adapted(2, 0); - - /* fill in new attitude data */ - R = R_adapted; - - /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; - } + vehicle_attitude_setpoint_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + _global_pos_sub.update(&_global_pos); + vehicle_status_poll(); + vehicle_land_detected_poll(); - const matrix::Eulerf euler_angles(R); + // the position controller will not emit attitude setpoints in some modes + // we need to make sure that this flag is reset + _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; - vehicle_attitude_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - _global_pos_sub.update(&_global_pos); - vehicle_status_poll(); - vehicle_land_detected_poll(); + /* lock integrator until control is started */ + bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode); - // the position controller will not emit attitude setpoints in some modes - // we need to make sure that this flag is reset - _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + /* Simple handling of failsafe: deploy parachute if failsafe is on */ + if (_vcontrol_mode.flag_control_termination_enabled) { + _actuators_airframe.control[7] = 1.0f; - /* lock integrator until control is started */ - bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled - || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode); - - /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_termination_enabled) { - _actuators_airframe.control[7] = 1.0f; + } else { + _actuators_airframe.control[7] = 0.0f; + } - } else { - _actuators_airframe.control[7] = 0.0f; - } + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { + perf_end(_loop_perf); + return; + } - /* if we are in rotary wing mode, do nothing */ - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) { - continue; - } + control_flaps(deltaT); - control_flaps(deltaT); + /* decide if in stabilized or full manual control */ + if (_vcontrol_mode.flag_control_rates_enabled) { - /* decide if in stabilized or full manual control */ - if (_vcontrol_mode.flag_control_rates_enabled) { + const float airspeed = get_airspeed_and_update_scaling(); - const float airspeed = get_airspeed_and_update_scaling(); + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + + _global_pos.vel_e * _global_pos.vel_e); + float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); - /* Use min airspeed to calculate ground speed scaling region. - * Don't scale below gspd_scaling_trim - */ - float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); - float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f); - float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); - - /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) { - _roll_ctrl.reset_integrator(); - } + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } - if (_att_sp.pitch_reset_integral) { - _pitch_ctrl.reset_integrator(); - } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } - if (_att_sp.yaw_reset_integral) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } - /* Reset integrators if the aircraft is on ground - * or a multicopter (but not transitioning VTOL) - */ - if (_landed - || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_vehicle_status.in_transition_mode)) { - - _roll_ctrl.reset_integrator(); - _pitch_ctrl.reset_integrator(); - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - } + /* Reset integrators if the aircraft is on ground + * or a multicopter (but not transitioning VTOL) + */ + if (_landed + || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode)) { + + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } - /* Prepare data for attitude controllers */ - struct ECL_ControlData control_input = {}; - control_input.roll = euler_angles.phi(); - control_input.pitch = euler_angles.theta(); - control_input.yaw = euler_angles.psi(); - control_input.body_x_rate = _att.rollspeed; - control_input.body_y_rate = _att.pitchspeed; - control_input.body_z_rate = _att.yawspeed; - control_input.roll_setpoint = _att_sp.roll_body; - control_input.pitch_setpoint = _att_sp.pitch_body; - control_input.yaw_setpoint = _att_sp.yaw_body; - control_input.airspeed_min = _parameters.airspeed_min; - control_input.airspeed_max = _parameters.airspeed_max; - control_input.airspeed = airspeed; - control_input.scaler = _airspeed_scaling; - control_input.lock_integrator = lock_integrator; - control_input.groundspeed = groundspeed; - control_input.groundspeed_scaler = groundspeed_scaler; - - /* reset body angular rate limits on mode change */ - if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { - if (_vcontrol_mode.flag_control_attitude_enabled - || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); - _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); - _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); - _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); + /* Prepare data for attitude controllers */ + struct ECL_ControlData control_input = {}; + control_input.roll = euler_angles.phi(); + control_input.pitch = euler_angles.theta(); + control_input.yaw = euler_angles.psi(); + control_input.body_x_rate = rollspeed; + control_input.body_y_rate = pitchspeed; + control_input.body_z_rate = yawspeed; + control_input.roll_setpoint = _att_sp.roll_body; + control_input.pitch_setpoint = _att_sp.pitch_body; + control_input.yaw_setpoint = _att_sp.yaw_body; + control_input.airspeed_min = _parameters.airspeed_min; + control_input.airspeed_max = _parameters.airspeed_max; + control_input.airspeed = airspeed; + control_input.scaler = _airspeed_scaling; + control_input.lock_integrator = lock_integrator; + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; + + /* reset body angular rate limits on mode change */ + if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { + if (_vcontrol_mode.flag_control_attitude_enabled + || _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); + _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); + _yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax)); - } else { - _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); - _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); - _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); - _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); - } + } else { + _roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad); + _pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad); + _pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad); + _yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad); } + } - _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; + _flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled; - /* bi-linear interpolation over airspeed for actuator trim scheduling */ - float trim_roll = _parameters.trim_roll; - float trim_pitch = _parameters.trim_pitch; - float trim_yaw = _parameters.trim_yaw; + /* bi-linear interpolation over airspeed for actuator trim scheduling */ + float trim_roll = _parameters.trim_roll; + float trim_pitch = _parameters.trim_pitch; + float trim_yaw = _parameters.trim_yaw; - if (airspeed < _parameters.airspeed_trim) { - trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin, - 0.0f); - trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin, - 0.0f); - trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin, - 0.0f); + if (airspeed < _parameters.airspeed_trim) { + trim_roll += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_roll_vmin, + 0.0f); + trim_pitch += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_pitch_vmin, + 0.0f); + trim_yaw += math::gradual(airspeed, _parameters.airspeed_min, _parameters.airspeed_trim, _parameters.dtrim_yaw_vmin, + 0.0f); - } else { - trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, - _parameters.dtrim_roll_vmax); - trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, - _parameters.dtrim_pitch_vmax); - trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, - _parameters.dtrim_yaw_vmax); - } + } else { + trim_roll += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, + _parameters.dtrim_roll_vmax); + trim_pitch += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, + _parameters.dtrim_pitch_vmax); + trim_yaw += math::gradual(airspeed, _parameters.airspeed_trim, _parameters.airspeed_max, 0.0f, + _parameters.dtrim_yaw_vmax); + } - /* add trim increment if flaps are deployed */ - trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps; - trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps; + /* add trim increment if flaps are deployed */ + trim_roll += _flaps_applied * _parameters.dtrim_roll_flaps; + trim_pitch += _flaps_applied * _parameters.dtrim_pitch_flaps; + + /* Run attitude controllers */ + if (_vcontrol_mode.flag_control_attitude_enabled) { + if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { + _roll_ctrl.control_attitude(control_input); + _pitch_ctrl.control_attitude(control_input); + _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + _wheel_ctrl.control_attitude(control_input); + + /* Update input data for rate controllers */ + control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); + control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); + control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_euler_rate(control_input); + _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - /* Run attitude controllers */ - if (_vcontrol_mode.flag_control_attitude_enabled) { - if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_attitude(control_input); - _pitch_ctrl.control_attitude(control_input); - _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _wheel_ctrl.control_attitude(control_input); - - /* Update input data for rate controllers */ - control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); - control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); - control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); - - /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - float roll_u = _roll_ctrl.control_euler_rate(control_input); - _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - - if (!PX4_ISFINITE(roll_u)) { - _roll_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - } + if (!PX4_ISFINITE(roll_u)) { + _roll_ctrl.reset_integrator(); + } - float pitch_u = _pitch_ctrl.control_euler_rate(control_input); - _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; + float pitch_u = _pitch_ctrl.control_euler_rate(control_input); + _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; - if (!PX4_ISFINITE(pitch_u)) { - _pitch_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - } + if (!PX4_ISFINITE(pitch_u)) { + _pitch_ctrl.reset_integrator(); + } - float yaw_u = 0.0f; + float yaw_u = 0.0f; - if (_parameters.w_en && _att_sp.fw_control_yaw) { - yaw_u = _wheel_ctrl.control_bodyrate(control_input); + if (_parameters.w_en && _att_sp.fw_control_yaw) { + yaw_u = _wheel_ctrl.control_bodyrate(control_input); - } else { - yaw_u = _yaw_ctrl.control_euler_rate(control_input); - } + } else { + yaw_u = _yaw_ctrl.control_euler_rate(control_input); + } - _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; + _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; - /* add in manual rudder control in manual modes */ - if (_vcontrol_mode.flag_control_manual_enabled) { - _actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r; - } + /* add in manual rudder control in manual modes */ + if (_vcontrol_mode.flag_control_manual_enabled) { + _actuators.control[actuator_controls_s::INDEX_YAW] += _manual.r; + } - if (!PX4_ISFINITE(yaw_u)) { - _yaw_ctrl.reset_integrator(); - _wheel_ctrl.reset_integrator(); - perf_count(_nonfinite_output_perf); - } + if (!PX4_ISFINITE(yaw_u)) { + _yaw_ctrl.reset_integrator(); + _wheel_ctrl.reset_integrator(); + } - /* throttle passed through if it is finite and if no engine failure was detected */ - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0]) - && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f; + /* throttle passed through if it is finite and if no engine failure was detected */ + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_body[0]) + && !_vehicle_status.engine_failure) ? _att_sp.thrust_body[0] : 0.0f; - /* scale effort by battery status */ - if (_parameters.bat_scale_en && - _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { + /* scale effort by battery status */ + if (_parameters.bat_scale_en && + _actuators.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) { - if (_battery_status_sub.updated()) { - battery_status_s battery_status{}; + if (_battery_status_sub.updated()) { + battery_status_s battery_status{}; - if (_battery_status_sub.copy(&battery_status)) { - if (battery_status.scale > 0.0f) { - _battery_scale = battery_status.scale; - } + if (_battery_status_sub.copy(&battery_status)) { + if (battery_status.scale > 0.0f) { + _battery_scale = battery_status.scale; } } - - _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; } - } else { - perf_count(_nonfinite_input_perf); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale; } + } - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - _rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); - _rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); - _rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); - - _rates_sp.timestamp = hrt_absolute_time(); + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + _rates_sp.roll = _roll_ctrl.get_desired_bodyrate(); + _rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate(); + _rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate(); - if (_rate_sp_pub != nullptr) { - /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); + _rates_sp.timestamp = hrt_absolute_time(); - } else { - /* advertise the attitude rates setpoint */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } + _rate_sp_pub.publish(_rates_sp); - } else { - vehicle_rates_setpoint_poll(); - - _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); - _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); - _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); + } else { + vehicle_rates_setpoint_poll(); - float roll_u = _roll_ctrl.control_bodyrate(control_input); - _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; + _roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll); + _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); + _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); - float pitch_u = _pitch_ctrl.control_bodyrate(control_input); - _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; + float roll_u = _roll_ctrl.control_bodyrate(control_input); + _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); - _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; + float pitch_u = _pitch_ctrl.control_bodyrate(control_input); + _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? - _rates_sp.thrust_body[0] : 0.0f; - } + float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; - rate_ctrl_status_s rate_ctrl_status; - rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed = _att.rollspeed; - rate_ctrl_status.pitchspeed = _att.pitchspeed; - rate_ctrl_status.yawspeed = _att.yawspeed; - rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); - rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); - rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); - rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); - - int instance; - orb_publish_auto(ORB_ID(rate_ctrl_status), &_rate_ctrl_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? + _rates_sp.thrust_body[0] : 0.0f; } - // Add feed-forward from roll control output to yaw control output - // This can be used to counteract the adverse yaw effect when rolling the plane - _actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain( - _actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); - - _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; - _actuators.control[5] = _manual.aux1; - _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; - // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future - _actuators.control[7] = _manual.aux3; - - /* lazily publish the setpoint only once available */ - _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; - _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; - - /* Only publish if any of the proper modes are enabled */ - if (_vcontrol_mode.flag_control_rates_enabled || - _vcontrol_mode.flag_control_attitude_enabled || - _vcontrol_mode.flag_control_manual_enabled) { - /* publish the actuator controls */ - if (_actuators_0_pub != nullptr) { - orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - - } else if (_actuators_id) { - _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); - } + rate_ctrl_status_s rate_ctrl_status; + rate_ctrl_status.timestamp = hrt_absolute_time(); + rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); + rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); + rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); + rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); - if (_actuators_2_pub != nullptr) { - /* publish the actuator controls*/ - orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); + _rate_ctrl_status_pub.publish(rate_ctrl_status); + } - } else { - /* advertise and publish */ - _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); - } + // Add feed-forward from roll control output to yaw control output + // This can be used to counteract the adverse yaw effect when rolling the plane + _actuators.control[actuator_controls_s::INDEX_YAW] += _parameters.roll_to_yaw_ff * math::constrain( + _actuators.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f); + + _actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied; + _actuators.control[5] = _manual.aux1; + _actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied; + // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future + _actuators.control[7] = _manual.aux3; + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _att.timestamp; + _actuators_airframe.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp_sample = _att.timestamp; + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_rates_enabled || + _vcontrol_mode.flag_control_attitude_enabled || + _vcontrol_mode.flag_control_manual_enabled) { + /* publish the actuator controls */ + if (_actuators_0_pub != nullptr) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + + } else if (_actuators_id) { + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } - } - perf_end(_loop_perf); + _actuators_2_pub.publish(_actuators_airframe); + } } + + perf_end(_loop_perf); } void FixedwingAttitudeControl::control_flaps(const float dt) @@ -914,26 +866,27 @@ void FixedwingAttitudeControl::control_flaps(const float dt) } } -FixedwingAttitudeControl *FixedwingAttitudeControl::instantiate(int argc, char *argv[]) -{ - return new FixedwingAttitudeControl(); -} - int FixedwingAttitudeControl::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("fw_att_controol", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL, - 1500, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; + FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - return 0; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } int FixedwingAttitudeControl::custom_command(int argc, char *argv[]) @@ -967,7 +920,8 @@ int FixedwingAttitudeControl::print_status() { PX4_INFO("Running"); - // perf? + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); return 0; } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index a93a94d809b9..651c206a67ec 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,13 +46,18 @@ #include #include #include +#include +#include +#include #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -66,7 +71,7 @@ using matrix::Quatf; using uORB::SubscriptionData; -class FixedwingAttitudeControl final : public ModuleBase +class FixedwingAttitudeControl final : public ModuleBase, public px4::WorkItem { public: FixedwingAttitudeControl(); @@ -75,24 +80,22 @@ class FixedwingAttitudeControl final : public ModuleBase _airspeed_sub{ORB_ID(airspeed)}; - orb_advert_t _rate_sp_pub{nullptr}; /**< rate setpoint publication */ - orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */ - orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_2_pub{nullptr}; /**< actuator control group 1 setpoint (Airframe) */ - orb_advert_t _rate_ctrl_status_pub{nullptr}; /**< rate controller status publication */ + uORB::Publication _actuators_2_pub{ORB_ID(actuator_controls_2)}; /**< actuator control group 1 setpoint (Airframe) */ + uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; /**< rate controller status publication */ + + orb_id_t _attitude_setpoint_id{nullptr}; + orb_advert_t _attitude_sp_pub{nullptr}; /**< attitude setpoint point */ - orb_id_t _actuators_id{nullptr}; // pointer to correct actuator controls0 uORB metadata structure - orb_id_t _attitude_setpoint_id{nullptr}; + orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + orb_advert_t _actuators_0_pub{nullptr}; /**< actuator control group 0 setpoint */ actuator_controls_s _actuators {}; /**< actuator control inputs */ actuator_controls_s _actuators_airframe {}; /**< actuator control inputs */ @@ -126,8 +131,7 @@ class FixedwingAttitudeControl final : public ModuleBase= min ground speed - * and positive if airspeed < min ground speed. - * * This error value ensures that a plane (as long as its throttle capability is * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = max(ground_speed_desired - ground_speed_body, 0.0f); - - } else { - _groundspeed_undershoot = 0.0f; + if (ground_speed_body < _groundspeed_min.get()) { + airspeed_demand += max(_groundspeed_min.get() - ground_speed_body, 0.0f); + } } + + // add minimum ground speed undershoot (only non-zero in presence of sufficient wind) + // sanity check: limit to range + return constrain(airspeed_demand, adjusted_min_airspeed, _parameters.airspeed_max); } void @@ -539,12 +531,7 @@ FixedwingPositionControl::tecs_status_publish() t.timestamp = hrt_absolute_time(); - if (_tecs_status_pub != nullptr) { - orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); - - } else { - _tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t); - } + _tecs_status_pub.publish(t); } void @@ -568,12 +555,7 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.timestamp = hrt_absolute_time(); - if (_pos_ctrl_status_pub != nullptr) { - orb_publish(ORB_ID(position_controller_status), _pos_ctrl_status_pub, &pos_ctrl_status); - - } else { - _pos_ctrl_status_pub = orb_advertise(ORB_ID(position_controller_status), &pos_ctrl_status); - } + _pos_ctrl_status_pub.publish(pos_ctrl_status); } void @@ -589,12 +571,7 @@ FixedwingPositionControl::landing_status_publish() pos_ctrl_landing_status.timestamp = hrt_absolute_time(); - if (_pos_ctrl_landing_status_pub != nullptr) { - orb_publish(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_pub, &pos_ctrl_landing_status); - - } else { - _pos_ctrl_landing_status_pub = orb_advertise(ORB_ID(position_controller_landing_status), &pos_ctrl_landing_status); - } + _pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status); } void @@ -774,8 +751,6 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF; // by default we don't use flaps - calculate_gndspeed_undershoot(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr); - Vector2f nav_speed_2d{ground_speed}; if (_airspeed_valid) { @@ -813,7 +788,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _tecs.reset_state(); } - if (_control_mode.flag_control_auto_enabled && pos_sp_curr.valid) { + if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) { /* AUTONOMOUS FLIGHT */ _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -883,7 +858,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(mission_airspeed), + calculate_target_airspeed(mission_airspeed, ground_speed), radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad, radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad, _parameters.throttle_min, @@ -931,7 +906,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto } tecs_update_pitch_throttle(alt_sp, - calculate_target_airspeed(mission_airspeed), + calculate_target_airspeed(mission_airspeed, ground_speed), radians(_parameters.pitch_limit_min) - _parameters.pitchsp_offset_rad, radians(_parameters.pitch_limit_max) - _parameters.pitchsp_offset_rad, _parameters.throttle_min, @@ -1016,7 +991,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1257,7 +1232,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min, ground_speed), radians(_parameters.pitch_limit_min), radians(takeoff_pitch_max_deg), _parameters.throttle_min, @@ -1292,7 +1267,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector } /* Detect launch using body X (forward) acceleration */ - _launchDetector.update(_sub_sensors.get().accel_x); + _launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]); /* update our copy of the launch detection state */ _launch_detection_state = _launchDetector.getLaunchDetected(); @@ -1343,7 +1318,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector } else { tecs_update_pitch_throttle(pos_sp_curr.alt, - calculate_target_airspeed(_parameters.airspeed_trim), + calculate_target_airspeed(_parameters.airspeed_trim, ground_speed), radians(_parameters.pitch_limit_min), radians(_parameters.pitch_limit_max), _parameters.throttle_min, @@ -1557,7 +1532,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector const float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, - calculate_target_airspeed(airspeed_land), + calculate_target_airspeed(airspeed_land, ground_speed), radians(_parameters.land_flare_pitch_min_deg), radians(_parameters.land_flare_pitch_max_deg), 0.0f, @@ -1625,7 +1600,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector const float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min; tecs_update_pitch_throttle(altitude_desired, - calculate_target_airspeed(airspeed_approach), + calculate_target_airspeed(airspeed_approach, ground_speed), radians(_parameters.pitch_limit_min), radians(_parameters.pitch_limit_max), _parameters.throttle_min, @@ -1673,41 +1648,19 @@ FixedwingPositionControl::handle_command() } void -FixedwingPositionControl::run() +FixedwingPositionControl::Run() { - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - - /* rate limit position updates to 50 Hz */ - orb_set_interval(_global_pos_sub, 20); - - /* abort on a nonzero return value from the parameter init */ - if (parameters_update() != PX4_OK) { - /* parameter setup went wrong, abort */ - PX4_ERR("aborting startup due to errors."); + if (should_exit()) { + _global_pos_sub.unregister_callback(); + exit_and_cleanup(); + return; } - /* wakeup source(s) */ - px4_pollfd_struct_t fds[1]; - - /* Setup of loop */ - fds[0].fd = _global_pos_sub; - fds[0].events = POLLIN; - - while (!should_exit()) { - - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - continue; - } + /* only run controller if position changed */ + if (_global_pos_sub.update(&_global_pos)) { /* only update parameters if they changed */ bool params_updated = _params_sub.updated(); @@ -1721,93 +1674,100 @@ FixedwingPositionControl::run() parameters_update(); } - /* only run controller if position changed */ - if ((fds[0].revents & POLLIN) != 0) { - perf_begin(_loop_perf); + _local_pos_sub.update(&_local_pos); - /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - _local_pos_sub.update(&_local_pos); + // handle estimator reset events. we only adjust setpoins for manual modes + if (_control_mode.flag_control_manual_enabled) { + if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) { + _hold_alt += _global_pos.delta_alt; + // make TECS accept step in altitude and demanded altitude + _tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt); + } - // handle estimator reset events. we only adjust setpoins for manual modes - if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.flag_control_altitude_enabled && _global_pos.alt_reset_counter != _alt_reset_counter) { - _hold_alt += _global_pos.delta_alt; - // make TECS accept step in altitude and demanded altitude - _tecs.handle_alt_step(_global_pos.delta_alt, _global_pos.alt); - } + // adjust navigation waypoints in position control mode + if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled + && _global_pos.lat_lon_reset_counter != _pos_reset_counter) { - // adjust navigation waypoints in position control mode - if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled - && _global_pos.lat_lon_reset_counter != _pos_reset_counter) { + // reset heading hold flag, which will re-initialise position control + _hdg_hold_enabled = false; + } + } - // reset heading hold flag, which will re-initialise position control - _hdg_hold_enabled = false; - } + // update the reset counters in any case + _alt_reset_counter = _global_pos.alt_reset_counter; + _pos_reset_counter = _global_pos.lat_lon_reset_counter; + + airspeed_poll(); + _manual_control_sub.update(&_manual); + _pos_sp_triplet_sub.update(&_pos_sp_triplet); + vehicle_attitude_poll(); + vehicle_command_poll(); + vehicle_control_mode_poll(); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + vehicle_status_poll(); + _vehicle_acceleration_sub.update(); + _vehicle_rates_sub.update(); + + Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); + Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); + + //Convert Local setpoints to global setpoints + if (_control_mode.flag_control_offboard_enabled) { + if (!globallocalconverter_initialized()) { + globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_alt, _local_pos.ref_timestamp); + + } else { + globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); } + } + + /* + * Attempt to control position, on success (= sensors present and not in manual mode), + * publish setpoint. + */ + if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) { + _att_sp.timestamp = hrt_absolute_time(); - // update the reset counters in any case - _alt_reset_counter = _global_pos.alt_reset_counter; - _pos_reset_counter = _global_pos.lat_lon_reset_counter; + // add attitude setpoint offsets + _att_sp.roll_body += _parameters.rollsp_offset_rad; + _att_sp.pitch_body += _parameters.pitchsp_offset_rad; - _sub_sensors.update(); - airspeed_poll(); - _manual_control_sub.update(&_manual); - _pos_sp_triplet_sub.update(&_pos_sp_triplet); - vehicle_attitude_poll(); - vehicle_command_poll(); - vehicle_control_mode_poll(); - _vehicle_land_detected_sub.update(&_vehicle_land_detected); - vehicle_status_poll(); + if (_control_mode.flag_control_manual_enabled) { + _att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad); + _att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad); + } - Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); - Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + _att_sp.q_d_valid = true; - /* - * Attempt to control position, on success (= sensors present and not in manual mode), - * publish setpoint. - */ - if (control_position(curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current, _pos_sp_triplet.next)) { - _att_sp.timestamp = hrt_absolute_time(); + if (_control_mode.flag_control_offboard_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_acceleration_enabled || + _control_mode.flag_control_altitude_enabled) { - // add attitude setpoint offsets - _att_sp.roll_body += _parameters.rollsp_offset_rad; - _att_sp.pitch_body += _parameters.pitchsp_offset_rad; + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub != nullptr) { + /* publish the attitude setpoint */ + orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -_parameters.man_roll_max_rad, _parameters.man_roll_max_rad); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, -_parameters.man_pitch_max_rad, _parameters.man_pitch_max_rad); + } else if (_attitude_setpoint_id != nullptr) { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); - q.copyTo(_att_sp.q_d); - _att_sp.q_d_valid = true; - - if (!_control_mode.flag_control_offboard_enabled || - _control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_acceleration_enabled) { - - /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp); - - } else if (_attitude_setpoint_id != nullptr) { - /* advertise and publish */ - _attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); - } - - // only publish status in full FW mode - if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_vehicle_status.in_transition_mode) { - status_publish(); - } + // only publish status in full FW mode + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_vehicle_status.in_transition_mode) { + status_publish(); } } - - perf_end(_loop_perf); } + + perf_end(_loop_perf); } } @@ -1930,7 +1890,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad; /* filter speed and altitude for controller */ - Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z); + Vector3f accel_body(_vehicle_acceleration_sub.get().xyz); // tailsitters use the multicopter frame as reference, in fixed wing // we need to use the fixed wing frame @@ -1943,6 +1903,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee /* tell TECS to update its state, but let it know when it cannot actually control the plane */ bool in_air_alt_control = (!_vehicle_land_detected.landed && (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_offboard_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_altitude_enabled)); @@ -1977,26 +1938,27 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee tecs_status_publish(); } -FixedwingPositionControl *FixedwingPositionControl::instantiate(int argc, char *argv[]) -{ - return new FixedwingPositionControl(); -} - int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("fw_pos_control_l1", - SCHED_DEFAULT, - SCHED_PRIORITY_POSITION_CONTROL, - 1810, - (px4_main_t)&run_trampoline, - (char *const *)argv); + FixedwingPositionControl *instance = new FixedwingPositionControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } - if (_task_id < 0) { - _task_id = -1; - return -errno; + } else { + PX4_ERR("alloc failed"); } - return 0; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } int FixedwingPositionControl::custom_command(int argc, char *argv[]) @@ -2029,6 +1991,9 @@ int FixedwingPositionControl::print_status() { PX4_INFO("Running"); + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 4f459574561f..c5364e2c18f4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -65,8 +65,10 @@ #include #include #include -#include +#include +#include #include +#include #include #include #include @@ -74,8 +76,9 @@ #include #include #include -#include #include +#include +#include #include #include #include @@ -122,28 +125,25 @@ static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; -class FixedwingPositionControl final : public ModuleBase, public ModuleParams +class FixedwingPositionControl final : public ModuleBase, public ModuleParams, + public px4::WorkItem { public: FixedwingPositionControl(); ~FixedwingPositionControl() override; - FixedwingPositionControl(const FixedwingPositionControl &) = delete; - FixedwingPositionControl operator=(const FixedwingPositionControl &other) = delete; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static FixedwingPositionControl *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + void Run() override; + + bool init(); /** @see ModuleBase::print_status() */ int print_status() override; @@ -151,7 +151,7 @@ class FixedwingPositionControl final : public ModuleBase _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */ - orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */ - orb_advert_t _pos_ctrl_landing_status_pub{nullptr}; ///< landing status publication */ - orb_advert_t _tecs_status_pub{nullptr}; ///< TECS status publication */ + orb_id_t _attitude_setpoint_id{nullptr}; - orb_id_t _attitude_setpoint_id{nullptr}; + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication */ + uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication */ + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication */ manual_control_setpoint_s _manual {}; ///< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items */ @@ -182,10 +183,11 @@ class FixedwingPositionControl final : public ModuleBase _sub_airspeed; - SubscriptionData _sub_sensors; + SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; + SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; ///< loop performance counter */ + perf_counter_t _loop_interval_perf; ///< loop interval performance counter */ float _hold_alt{0.0f}; ///< hold altitude for altitude mode */ float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched */ @@ -371,6 +373,9 @@ class FixedwingPositionControl final : public ModuleBase) _groundspeed_min + ) // Update our local parameter cache. int parameters_update(); @@ -436,9 +441,7 @@ class FixedwingPositionControl final : public ModuleBase */ -#include "FixedwingLandDetector.h" - -#include +#include -#include -#include +#include "FixedwingLandDetector.h" namespace land_detector { @@ -59,7 +56,7 @@ FixedwingLandDetector::FixedwingLandDetector() void FixedwingLandDetector::_update_topics() { _airspeed_sub.update(&_airspeed); - _sensor_bias_sub.update(&_sensor_bias); + _vehicle_acceleration_sub.update(&_vehicle_acceleration); _vehicle_local_position_sub.update(&_vehicle_local_position); } @@ -89,7 +86,7 @@ bool FixedwingLandDetector::_get_landed_state() bool landDetected = false; - if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500 * 1000) { + if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms) { // Horizontal velocity complimentary filter. float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + @@ -110,13 +107,13 @@ bool FixedwingLandDetector::_get_landed_state() // A leaking lowpass prevents biases from building up, but // gives a mostly correct response for short impulses. - const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x + - _sensor_bias.accel_y * _sensor_bias.accel_y); + const matrix::Vector3f accel{_vehicle_acceleration.xyz}; + const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1)); - _accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f; + _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; // crude land detector for fixedwing - landDetected = _accel_horz_lp < _param_lndfw_xyaccel_max.get() + landDetected = _xy_accel_filtered < _param_lndfw_xyaccel_max.get() && _airspeed_filtered < _param_lndfw_airspd.get() && _velocity_xy_filtered < _param_lndfw_vel_xy_max.get() && _velocity_z_filtered < _param_lndfw_vel_z_max.get(); diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index cdfa44f28a1f..020a769e2028 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,10 +42,9 @@ #pragma once -#include #include #include -#include +#include #include #include "LandDetector.h" @@ -75,17 +74,17 @@ class FixedwingLandDetector final : public LandDetector uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; airspeed_s _airspeed{}; - sensor_bias_s _sensor_bias{}; + vehicle_acceleration_s _vehicle_acceleration{}; vehicle_local_position_s _vehicle_local_position{}; - float _accel_horz_lp{0.0f}; float _airspeed_filtered{0.0f}; float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; + float _xy_accel_filtered{0.0f}; DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 36d90def5dd9..41dc9df878cc 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -42,16 +42,16 @@ #pragma once -#include -#include #include #include #include +#include +#include +#include #include #include #include #include -#include using namespace time_literals; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index e5228ce2942e..4348a47ebd9c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -63,6 +63,7 @@ #include #include +#include #include "MulticopterLandDetector.h" @@ -72,70 +73,55 @@ namespace land_detector MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); - _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); - _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); - _paramHandle.minThrottle = param_find("MPC_THR_MIN"); - _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); - _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR"); - _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); - _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); - _paramHandle.low_thrust_threshold = param_find("LNDMC_LOW_T_THR"); + _paramHandle.minThrottle = param_find("MPC_THR_MIN"); + _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). + _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); _landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US); _maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US); - _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); } void MulticopterLandDetector::_update_topics() { _actuator_controls_sub.update(&_actuator_controls); _battery_sub.update(&_battery_status); - _sensor_bias_sub.update(&_sensor_bias); + _vehicle_acceleration_sub.update(&_vehicle_acceleration); + _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); _vehicle_attitude_sub.update(&_vehicle_attitude); - _vehicle_control_mode_sub.update(&_control_mode); + _vehicle_control_mode_sub.update(&_vehicle_control_mode); _vehicle_local_position_sub.update(&_vehicle_local_position); _vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint); } void MulticopterLandDetector::_update_params() { - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); - _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); + _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get())); + param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold); - param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time); - _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); - param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.landSpeed, &_params.landSpeed); - param_get(_paramHandle.low_thrust_threshold, &_params.low_thrust_threshold); } bool MulticopterLandDetector::_get_freefall_state() { - if (_params.freefall_acc_threshold < 0.1f - || _params.freefall_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. + if (_param_lndmc_ffall_thr.get() < 0.1f || + _param_lndmc_ffall_thr.get() > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. return false; } - if (_sensor_bias.timestamp == 0) { + if (_vehicle_acceleration.timestamp == 0) { // _sensors is not valid yet, we have to assume we're not falling. return false; } - float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x - + _sensor_bias.accel_y * _sensor_bias.accel_y - + _sensor_bias.accel_z * _sensor_bias.accel_z; - acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed. + // norm of specific force. Should be close to 9.8 m/s^2 when landed. + const matrix::Vector3f accel{_vehicle_acceleration.xyz}; - return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling + return (accel.norm() < _param_lndmc_ffall_thr.get()); // true if we are currently falling } bool MulticopterLandDetector::_get_ground_contact_state() @@ -151,35 +137,35 @@ bool MulticopterLandDetector::_get_ground_contact_state() // Check if we are moving vertically - this might see a spike after arming due to // throttle-up vibration. If accelerating fast the throttle thresholds will still give // an accurate in-air indication. - bool verticalMovement; + bool vertical_movement = false; if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) { // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - verticalMovement = fabsf(_vehicle_local_position.vz) > _params.maxClimbRate * 2.5f; + vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f; } else { - // Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate - float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) : - _params.maxClimbRate; - verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate; + // Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate + float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) : + _param_lndmc_z_vel_max.get(); + vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate; } // Check if we are moving horizontally. _horizontal_movement = sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx - + _vehicle_local_position.vy * _vehicle_local_position.vy) > _params.maxVelocity; + + _vehicle_local_position.vy * _vehicle_local_position.vy) > _param_lndmc_xy_vel_max.get(); // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, // we then can assume that the vehicle hit ground _in_descend = _is_climb_rate_enabled() && (_vehicle_local_position_setpoint.vz >= land_speed_threshold); - bool hit_ground = _in_descend && !verticalMovement; + bool hit_ground = _in_descend && !vertical_movement; // TODO: we need an accelerometer based check for vertical movement for flying without GPS if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock()) - && (!verticalMovement || !_has_altitude_lock())) { + && (!vertical_movement || !_has_altitude_lock())) { return true; } @@ -192,7 +178,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() const hrt_abstime now = hrt_absolute_time(); // When not armed, consider to be maybe-landed - if (!_actuator_armed.armed) { + if (!_actuator_armed.armed || (_vehicle_attitude.timestamp == 0)) { return true; } @@ -213,11 +199,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state() } // Next look if all rotation angles are not moving. - float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor; + float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; - bool rotating = (fabsf(_vehicle_attitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicle_attitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicle_attitude.yawspeed) > maxRotationScaled); + bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) || + (fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) || + (fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled); // Return status based on armed state and throttle if no position lock is available. if (!_has_altitude_lock() && !rotating) { @@ -262,19 +248,23 @@ bool MulticopterLandDetector::_get_landed_state() float MulticopterLandDetector::_get_max_altitude() { - /* ToDo: add a meaningful altitude */ - float valid_altitude_max = _params.altitude_max; + /* TODO: add a meaningful altitude */ + float valid_altitude_max = _param_lndmc_alt_max.get(); + + if (valid_altitude_max < 0.0f) { + return INFINITY; + } if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) { - valid_altitude_max = _params.altitude_max * 0.75f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f; } if (_battery_status.warning == battery_status_s::BATTERY_WARNING_CRITICAL) { - valid_altitude_max = _params.altitude_max * 0.5f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.5f; } if (_battery_status.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { - valid_altitude_max = _params.altitude_max * 0.25f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.25f; } return valid_altitude_max; @@ -297,7 +287,7 @@ bool MulticopterLandDetector::_is_climb_rate_enabled() bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0) && (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms); - return (_control_mode.flag_control_climb_rate_enabled && has_updated + return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated && PX4_ISFINITE(_vehicle_local_position_setpoint.vz)); } @@ -305,7 +295,7 @@ bool MulticopterLandDetector::_has_low_thrust() { // 30% of throttle range between min and hover float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * - _params.low_thrust_threshold; + _param_lndmc_low_t_thr.get(); // Check if thrust output is less than the minimum auto throttle param. return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle; @@ -317,7 +307,7 @@ bool MulticopterLandDetector::_has_minimal_thrust() float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f; // Determine the system min throttle based on flight mode - if (!_control_mode.flag_control_climb_rate_enabled) { + if (!_vehicle_control_mode.flag_control_climb_rate_enabled) { sys_min_throttle = (_params.minManThrottle + 0.01f); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 19ec57dab0a5..7219ca7f9576 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -42,20 +42,19 @@ #pragma once -#include "LandDetector.h" +#include -#include -#include -#include -#include -#include #include #include #include -#include +#include +#include #include #include #include +#include + +#include "LandDetector.h" using namespace time_literals; @@ -80,6 +79,15 @@ class MulticopterLandDetector : public LandDetector float _get_max_altitude() override; private: + /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ + float _get_takeoff_throttle(); + + bool _has_low_thrust(); + bool _has_minimal_thrust(); + bool _has_altitude_lock(); + bool _has_position_lock(); + bool _is_climb_rate_enabled(); + /** Time in us that landing conditions have to hold before triggering a land. */ static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms; @@ -96,36 +104,23 @@ class MulticopterLandDetector : public LandDetector * @brief Handles for interesting parameters **/ struct { - param_t maxClimbRate; - param_t maxVelocity; - param_t maxRotation; param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t freefall_acc_threshold; - param_t freefall_trigger_time; - param_t altitude_max; param_t landSpeed; - param_t low_thrust_threshold; } _paramHandle{}; struct { - float maxClimbRate; - float maxVelocity; - float maxRotation_rad_s; float minThrottle; float hoverThrottle; float minManThrottle; - float freefall_acc_threshold; - float freefall_trigger_time; - float altitude_max; float landSpeed; - float low_thrust_threshold; } _params{}; uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _battery_sub{ORB_ID(battery_status)}; - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -133,26 +128,29 @@ class MulticopterLandDetector : public LandDetector actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; - vehicle_control_mode_s _control_mode {}; - sensor_bias_s _sensor_bias {}; + vehicle_acceleration_s _vehicle_acceleration{}; + vehicle_angular_velocity_s _vehicle_angular_velocity{}; vehicle_attitude_s _vehicle_attitude {}; + vehicle_control_mode_s _vehicle_control_mode {}; vehicle_local_position_s _vehicle_local_position {}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; - hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first + hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; - bool _in_descend{false}; ///< vehicle is desending + bool _in_descend{false}; ///< vehicle is desending bool _horizontal_movement{false}; ///< vehicle is moving horizontally - /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ - float _get_takeoff_throttle(); - bool _has_altitude_lock(); - bool _has_position_lock(); - bool _has_minimal_thrust(); - bool _has_low_thrust(); - bool _is_climb_rate_enabled(); + DEFINE_PARAMETERS_CUSTOM_PARENT( + LandDetector, + (ParamFloat) _param_lndmc_alt_max, + (ParamFloat) _param_lndmc_ffall_thr, + (ParamFloat) _param_lndmc_ffall_ttri, + (ParamFloat) _param_lndmc_low_t_thr, + (ParamFloat) _param_lndmc_rot_max, + (ParamFloat) _param_lndmc_xy_vel_max, + (ParamFloat) _param_lndmc_z_vel_max + ); }; - } // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp index 99c77bddfca1..407bf47f6f21 100644 --- a/src/modules/land_detector/VtolLandDetector.cpp +++ b/src/modules/land_detector/VtolLandDetector.cpp @@ -48,22 +48,20 @@ namespace land_detector VtolLandDetector::VtolLandDetector() { - _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } void VtolLandDetector::_update_topics() { MulticopterLandDetector::_update_topics(); - _airspeed_sub.update(&_airspeed); _vehicle_status_sub.update(&_vehicle_status); } bool VtolLandDetector::_get_maybe_landed_state() { - // Only trigger in RW mode + // If in Fixed-wing mode, only trigger if disarmed if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return false; + return !_actuator_armed.armed; } return MulticopterLandDetector::_get_maybe_landed_state(); @@ -71,9 +69,9 @@ bool VtolLandDetector::_get_maybe_landed_state() bool VtolLandDetector::_get_landed_state() { - // Only trigger in RW mode + // If in Fixed-wing mode, only trigger if disarmed if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - return false; + return !_actuator_armed.armed; } // this is returned from the mutlicopter land detector @@ -92,7 +90,7 @@ bool VtolLandDetector::_get_landed_state() // only consider airspeed if we have been in air before to avoid false // detections in the case of wind on the ground - if (_was_in_air && (_airspeed_filtered > _params.maxAirSpeed)) { + if (_was_in_air && (_airspeed_filtered > _param_lndfw_airspd_max.get())) { landed = false; } @@ -101,11 +99,4 @@ bool VtolLandDetector::_get_landed_state() return landed; } -void VtolLandDetector::_update_params() -{ - MulticopterLandDetector::_update_params(); - - param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); -} - } // namespace land_detector diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h index 47f29f646f89..d54541963a0c 100644 --- a/src/modules/land_detector/VtolLandDetector.h +++ b/src/modules/land_detector/VtolLandDetector.h @@ -56,19 +56,11 @@ class VtolLandDetector : public MulticopterLandDetector VtolLandDetector(); protected: - void _update_params() override; void _update_topics() override; bool _get_landed_state() override; bool _get_maybe_landed_state() override; private: - struct { - param_t maxAirSpeed; - } _paramHandle{}; - - struct { - float maxAirSpeed; - } _params{}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -78,7 +70,11 @@ class VtolLandDetector : public MulticopterLandDetector bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */ float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */ -}; + DEFINE_PARAMETERS_CUSTOM_PARENT( + MulticopterLandDetector, + (ParamFloat) _param_lndfw_airspd_max + ); +}; } // namespace land_detector diff --git a/src/modules/landing_target_estimator/CMakeLists.txt b/src/modules/landing_target_estimator/CMakeLists.txt index b8b8c40755c3..146869aa3fe4 100644 --- a/src/modules/landing_target_estimator/CMakeLists.txt +++ b/src/modules/landing_target_estimator/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE modules__landing_target_estimator MAIN landing_target_estimator - STACK_MAIN 1200 COMPILE_FLAGS SRCS landing_target_estimator_main.cpp diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 61f48d427841..027a8627e00f 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -80,14 +80,11 @@ void LandingTargetEstimator::update() float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC; // predict target position with the help of accel data - matrix::Vector3f a; + matrix::Vector3f a{_vehicle_acceleration.xyz}; - if (_vehicleAttitude_valid && _sensorBias_valid) { + if (_vehicleAttitude_valid && _vehicle_acceleration_valid) { matrix::Quaternion q_att(&_vehicleAttitude.q[0]); _R_att = matrix::Dcm(q_att); - a(0) = _sensorBias.accel_x; - a(1) = _sensorBias.accel_y; - a(2) = _sensorBias.accel_z; a = _R_att * a; } else { @@ -244,7 +241,7 @@ void LandingTargetEstimator::_update_topics() { _vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition); _vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude); - _sensorBias_valid = _sensorBiasSub.update(&_sensorBias); + _vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration); _new_irlockReport = _irlockReportSub.update(&_irlockReport); } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 45062a24085d..ef4a12061756 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -47,9 +47,9 @@ #include #include #include -#include +#include #include -#include +#include #include #include #include @@ -130,18 +130,18 @@ class LandingTargetEstimator uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)}; uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; vehicle_local_position_s _vehicleLocalPosition{}; - vehicle_attitude_s _vehicleAttitude{}; - sensor_bias_s _sensorBias{}; - irlock_report_s _irlockReport{}; + vehicle_attitude_s _vehicleAttitude{}; + vehicle_acceleration_s _vehicle_acceleration{}; + irlock_report_s _irlockReport{}; // keep track of which topics we have received bool _vehicleLocalPosition_valid{false}; bool _vehicleAttitude_valid{false}; - bool _sensorBias_valid{false}; + bool _vehicle_acceleration_valid{false}; bool _new_irlockReport{false}; bool _estimator_initialized{false}; // keep track of whether last measurement was rejected diff --git a/src/modules/load_mon/CMakeLists.txt b/src/modules/load_mon/CMakeLists.txt index 52d43507159c..d98108ca2dc9 100644 --- a/src/modules/load_mon/CMakeLists.txt +++ b/src/modules/load_mon/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE modules__load_mon MAIN load_mon - STACK_MAIN 1200 COMPILE_FLAGS SRCS load_mon.cpp diff --git a/src/modules/load_mon/load_mon.cpp b/src/modules/load_mon/load_mon.cpp index 7781ffc71f84..7f4e10b0de7b 100644 --- a/src/modules/load_mon/load_mon.cpp +++ b/src/modules/load_mon/load_mon.cpp @@ -45,11 +45,12 @@ #include #include #include -#include +#include #include +#include +#include #include #include -#include #if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION) # error load_mon support requires CONFIG_SCHED_INSTRUMENTATION @@ -105,14 +106,14 @@ class LoadMon : public ModuleBase, public ModuleParams, public px4::Sch void _stack_usage(); int _stack_task_index{0}; - orb_advert_t _task_stack_info_pub{nullptr}; + uORB::PublicationQueued _task_stack_info_pub{ORB_ID(task_stack_info)}; #endif DEFINE_PARAMETERS( (ParamBool) _param_sys_stck_en ) - orb_advert_t _cpuload_pub{nullptr}; + uORB::Publication _cpuload_pub{ORB_ID(cpuload)}; hrt_abstime _last_idle_time{0}; hrt_abstime _last_idle_time_sample{0}; @@ -192,17 +193,12 @@ void LoadMon::_cpuload() _last_idle_time = total_runtime; _last_idle_time_sample = hrt_absolute_time(); - cpuload_s cpuload = {}; + cpuload_s cpuload{}; cpuload.load = 1.0f - (float)interval_idletime / (float)interval; cpuload.ram_usage = _ram_used(); cpuload.timestamp = hrt_absolute_time(); - if (_cpuload_pub == nullptr) { - _cpuload_pub = orb_advertise(ORB_ID(cpuload), &cpuload); - - } else { - orb_publish(ORB_ID(cpuload), _cpuload_pub, &cpuload); - } + _cpuload_pub.publish(cpuload); } float LoadMon::_ram_used() @@ -284,12 +280,7 @@ void LoadMon::_stack_usage() task_stack_info.stack_free = stack_free; task_stack_info.timestamp = hrt_absolute_time(); - if (_task_stack_info_pub == nullptr) { - _task_stack_info_pub = orb_advertise_queue(ORB_ID(task_stack_info), &task_stack_info, num_tasks_per_cycle); - - } else { - orb_publish(ORB_ID(task_stack_info), _task_stack_info_pub, &task_stack_info); - } + _task_stack_info_pub.publish(task_stack_info); /* * Found task low on stack, report and exit. Continue here in next cycle. diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 356dd120ca63..0451119b1799 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -25,6 +25,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()), _sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()), _sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()), + _sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()), // set flow max update rate higher than expected to we don't lose packets _sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()), // main prediction loop, 100 hz @@ -663,9 +664,9 @@ void BlockLocalPositionEstimator::publishOdom() _pub_odom.get().vz = xLP(X_vz); // vel down // angular velocity - _pub_odom.get().rollspeed = _sub_att.get().rollspeed; // roll rate - _pub_odom.get().pitchspeed = _sub_att.get().pitchspeed; // pitch rate - _pub_odom.get().yawspeed = _sub_att.get().yawspeed; // yaw rate + _pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate + _pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate + _pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate // get the covariance matrix size const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index c38e073fa364..ef6126e2f7aa 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -249,6 +250,7 @@ class BlockLocalPositionEstimator : public control::SuperBlock, public ModulePar uORB::SubscriptionPollable _sub_armed; uORB::SubscriptionPollable _sub_land; uORB::SubscriptionPollable _sub_att; + uORB::SubscriptionPollable _sub_angular_velocity; uORB::SubscriptionPollable _sub_flow; uORB::SubscriptionPollable _sub_sensor; uORB::SubscriptionPollable _sub_param_update; diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index c6fd629605f6..60b0c03ff9e2 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -152,9 +152,10 @@ void BlockLocalPositionEstimator::flowCorrect() // compute polynomial value float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h; - float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed - + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed - + _sub_att.get().yawspeed * _sub_att.get().yawspeed; + const Vector3f rates{_sub_angular_velocity.get().xyz}; + float rotrate_sq = rates(0) * rates(0) + + rates(1) * rates(1) + + rates(2) * rates(2); matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta(); diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt index ef5e4eab62cf..5949f50e0b45 100644 --- a/src/modules/logger/CMakeLists.txt +++ b/src/modules/logger/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MODULE modules__logger MAIN logger PRIORITY "SCHED_PRIORITY_MAX-30" - STACK_MAIN 2200 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index 5f68a98e280b..8511f9e92a3d 100644 --- a/src/modules/logger/log_writer.cpp +++ b/src/modules/logger/log_writer.cpp @@ -38,7 +38,7 @@ namespace px4 namespace logger { -LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size, unsigned int queue_size) +LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size) : _backend(configured_backend) { if (configured_backend & BackendFile) { @@ -50,7 +50,7 @@ LogWriter::LogWriter(Backend configured_backend, size_t file_buffer_size, unsign } if (configured_backend & BackendMavlink) { - _log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(queue_size); + _log_writer_mavlink_for_write = _log_writer_mavlink = new LogWriterMavlink(); if (!_log_writer_mavlink) { PX4_ERR("LogWriterMavlink allocation failed"); diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 7aa9c1241747..abac8a5d57c9 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -55,7 +55,7 @@ class LogWriter static constexpr Backend BackendMavlink = 1 << 1; static constexpr Backend BackendAll = BackendFile | BackendMavlink; - LogWriter(Backend configured_backend, size_t file_buffer_size, unsigned int queue_size); + LogWriter(Backend configured_backend, size_t file_buffer_size); ~LogWriter(); bool init(); diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index b99eafd0a49d..d0b0c2bc330e 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -45,9 +45,7 @@ namespace px4 namespace logger { - -LogWriterMavlink::LogWriterMavlink(unsigned int queue_size) : - _queue_size(queue_size) +LogWriterMavlink::LogWriterMavlink() { _ulog_stream_data.length = 0; } @@ -62,21 +60,10 @@ LogWriterMavlink::~LogWriterMavlink() if (_ulog_stream_ack_sub >= 0) { orb_unsubscribe(_ulog_stream_ack_sub); } - - if (_ulog_stream_pub) { - orb_unadvertise(_ulog_stream_pub); - } } void LogWriterMavlink::start_log() { - if (_ulog_stream_pub == nullptr) { - memset(&_ulog_stream_data, 0, sizeof(_ulog_stream_data)); - // advertise before we subscribe: this is a trick to reduce the number of needed file descriptors - // (the advertise temporarily opens a file descriptor) - _ulog_stream_pub = orb_advertise_queue(ORB_ID(ulog_stream), &_ulog_stream_data, _queue_size); - } - if (_ulog_stream_ack_sub == -1) { _ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack)); } @@ -84,9 +71,11 @@ void LogWriterMavlink::start_log() // make sure we don't get any stale ack's by doing an orb_copy ulog_stream_ack_s ack; orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack); + _ulog_stream_data.sequence = 0; _ulog_stream_data.length = 0; _ulog_stream_data.first_message_offset = 0; + _is_started = true; } @@ -147,12 +136,7 @@ int LogWriterMavlink::publish_message() _ulog_stream_data.flags = _ulog_stream_data.FLAGS_NEED_ACK; } - if (_ulog_stream_pub == nullptr) { - _ulog_stream_pub = orb_advertise_queue(ORB_ID(ulog_stream), &_ulog_stream_data, _queue_size); - - } else { - orb_publish(ORB_ID(ulog_stream), _ulog_stream_pub, &_ulog_stream_data); - } + _ulog_stream_pub.publish(_ulog_stream_data); if (_need_reliable_transfer) { // we need to wait for an ack. Note that this blocks the main logger thread, so if a file logging diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h index 5a086ebb8199..6b6f36819808 100644 --- a/src/modules/logger/log_writer_mavlink.h +++ b/src/modules/logger/log_writer_mavlink.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -49,7 +50,7 @@ namespace logger class LogWriterMavlink { public: - LogWriterMavlink(unsigned int queue_size); + LogWriterMavlink(); ~LogWriterMavlink(); bool init(); @@ -75,12 +76,11 @@ class LogWriterMavlink /** publish message, wait for ack if needed & reset message */ int publish_message(); - ulog_stream_s _ulog_stream_data; - orb_advert_t _ulog_stream_pub = nullptr; - int _ulog_stream_ack_sub = -1; - bool _need_reliable_transfer = false; - bool _is_started = false; - const unsigned int _queue_size; + ulog_stream_s _ulog_stream_data{}; + uORB::PublicationQueued _ulog_stream_pub{ORB_ID(ulog_stream)}; + int _ulog_stream_ack_sub{-1}; + bool _need_reliable_transfer{false}; + bool _is_started{false}; }; } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index f3f986dfebab..6bfa28a32ebe 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -44,14 +44,10 @@ #include #include -#include +#include #include -#include -#include #include -#include #include -#include #include #include @@ -243,8 +239,6 @@ Logger *Logger::instantiate(int argc, char *argv[]) Logger::LogMode log_mode = Logger::LogMode::while_armed; bool error_flag = false; bool log_name_timestamp = false; - unsigned int queue_size = 14; //TODO: we might be able to reduce this if mavlink polled on the topic and/or - // topic sizes get reduced LogWriter::Backend backend = LogWriter::BackendAll; const char *poll_topic = nullptr; @@ -252,7 +246,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "r:b:etfm:q:p:x", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "r:b:etfm:p:x", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(myoptarg, nullptr, 10); @@ -314,15 +308,6 @@ Logger *Logger::instantiate(int argc, char *argv[]) poll_topic = myoptarg; break; - case 'q': - queue_size = strtoul(myoptarg, nullptr, 10); - - if (queue_size == 0) { - queue_size = 1; - } - - break; - case '?': error_flag = true; break; @@ -347,8 +332,7 @@ Logger *Logger::instantiate(int argc, char *argv[]) return nullptr; } - Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp, - queue_size); + Logger *logger = new Logger(backend, log_buffer_size, log_interval, poll_topic, log_mode, log_name_timestamp); #if defined(DBGPRINT) && defined(__PX4_NUTTX) struct mallinfo alloc_info = mallinfo(); @@ -377,10 +361,10 @@ Logger *Logger::instantiate(int argc, char *argv[]) Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - LogMode log_mode, bool log_name_timestamp, unsigned int queue_size) : + LogMode log_mode, bool log_name_timestamp) : _log_mode(log_mode), _log_name_timestamp(log_name_timestamp), - _writer(backend, buffer_size, queue_size), + _writer(backend, buffer_size), _log_interval(log_interval) { _log_utc_offset = param_find("SDLOG_UTC_OFFSET"); @@ -425,7 +409,7 @@ bool Logger::request_stop_static() return true; } -LoggerSubscription *Logger::add_topic(const orb_metadata *topic) +LoggerSubscription *Logger::add_topic(const orb_metadata *topic, uint32_t interval_ms, uint8_t instance) { LoggerSubscription *subscription = nullptr; size_t fields_len = strlen(topic->o_fields) + strlen(topic->o_name) + 1; //1 for ':' @@ -437,38 +421,23 @@ LoggerSubscription *Logger::add_topic(const orb_metadata *topic) return nullptr; } - int fd = -1; - - // Only subscribe to the topic now if it's published. If published later on, we'll dynamically - // add the subscription then - if (orb_exists(topic, 0) == 0) { - fd = orb_subscribe(topic); - - if (fd < 0) { - PX4_WARN("logger: %s subscribe failed (%i)", topic->o_name, errno); - return nullptr; - } - - } else { - PX4_DEBUG("Topic %s does not exist. Not subscribing (yet)", topic->o_name); - } - - if (_subscriptions.push_back(LoggerSubscription(fd, topic))) { + if (_subscriptions.push_back(LoggerSubscription{topic, interval_ms, instance})) { subscription = &_subscriptions[_subscriptions.size() - 1]; } else { - PX4_WARN("logger: failed to add topic. Too many subscriptions"); - - if (fd >= 0) { - orb_unsubscribe(fd); - } + PX4_WARN("Too many subscriptions, failed to add: %s %d", topic->o_name, instance); } return subscription; } -bool Logger::add_topic(const char *name, unsigned interval) +bool Logger::add_topic(const char *name, uint32_t interval_ms, uint8_t instance) { + // if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval + if (_polling_topic_meta) { + interval_ms = 0; + } + const orb_metadata *const *topics = orb_get_topics(); LoggerSubscription *subscription = nullptr; @@ -478,9 +447,13 @@ bool Logger::add_topic(const char *name, unsigned interval) // check if already added: if so, only update the interval for (size_t j = 0; j < _subscriptions.size(); ++j) { - if (_subscriptions[j].metadata == topics[i]) { - PX4_DEBUG("logging topic %s, interval: %i, already added, only setting interval", - topics[i]->o_name, interval); + if ((_subscriptions[j].get_topic() == topics[i]) && (_subscriptions[j].get_instance() == instance)) { + + PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval", + topics[i]->o_name, instance, interval_ms); + + _subscriptions[j].set_interval_ms(interval_ms); + subscription = &_subscriptions[j]; already_added = true; break; @@ -488,164 +461,119 @@ bool Logger::add_topic(const char *name, unsigned interval) } if (!already_added) { - subscription = add_topic(topics[i]); - PX4_DEBUG("logging topic: %s, interval: %i", topics[i]->o_name, interval); + subscription = add_topic(topics[i], interval_ms, instance); + PX4_DEBUG("logging topic: %s(%d), interval: %i", topics[i]->o_name, instance, interval_ms); break; } } } - // if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval - if (_polling_topic_meta) { - interval = 0; - } - - if (subscription) { - if (subscription->fd[0] >= 0) { - orb_set_interval(subscription->fd[0], interval); + return (subscription != nullptr); +} - } else { - // store the interval: use a value < 0 to know it's not a valid fd - subscription->fd[0] = -interval - 1; - } +bool Logger::add_topic_multi(const char *name, uint32_t interval_ms) +{ + // add all possible instances + for (uint8_t instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { + add_topic(name, interval_ms, instance); } - return subscription; + return true; } -bool Logger::copy_if_updated_multi(int sub_idx, int multi_instance, void *buffer, bool try_to_subscribe) +bool Logger::copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe) { - bool updated = false; LoggerSubscription &sub = _subscriptions[sub_idx]; - int &handle = sub.fd[multi_instance]; - if (handle < 0 && try_to_subscribe) { + bool updated = false; - if (try_to_subscribe_topic(sub, multi_instance)) { + if (sub.valid()) { + updated = sub.update(buffer); - write_add_logged_msg(LogType::Full, sub, multi_instance); + } else if (try_to_subscribe) { + if (sub.subscribe()) { + write_add_logged_msg(LogType::Full, sub); if (sub_idx < _num_mission_subs) { - write_add_logged_msg(LogType::Mission, sub, multi_instance); + write_add_logged_msg(LogType::Mission, sub); } - /* copy first data */ - if (orb_copy(sub.metadata, handle, buffer) == PX4_OK) { - updated = true; - } - } - - } else if (handle >= 0) { - orb_check(handle, &updated); - - if (updated) { - orb_copy(sub.metadata, handle, buffer); + // copy first data + updated = sub.copy(buffer); } } return updated; } -bool Logger::try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance) -{ - bool ret = false; - - if (OK == orb_exists(sub.metadata, multi_instance)) { - - unsigned int interval; - - if (multi_instance == 0) { - // the first instance and no subscription yet: this means we stored the negative interval as fd - interval = (unsigned int)(-(sub.fd[0] + 1)); - - } else { - // set to the same interval as the first instance - if (orb_get_interval(sub.fd[0], &interval) != 0) { - interval = 0; - } - } - - int &handle = sub.fd[multi_instance]; - handle = orb_subscribe_multi(sub.metadata, multi_instance); - - if (handle >= 0) { - PX4_DEBUG("subscribed to instance %d of topic %s", multi_instance, sub.metadata->o_name); - - if (interval > 0) { - orb_set_interval(handle, interval); - } - - ret = true; - - } else { - PX4_ERR("orb_subscribe_multi %s failed (%i)", sub.metadata->o_name, errno); - } - } - - return ret; -} - void Logger::add_default_topics() { - // Note: try to avoid setting the interval where possible, as it increases RAM usage add_topic("actuator_controls_0", 100); add_topic("actuator_controls_1", 100); - add_topic("actuator_outputs", 100); add_topic("airspeed", 200); - add_topic("battery_status", 500); + add_topic("airspeed_validated", 200); add_topic("camera_capture"); add_topic("camera_trigger"); add_topic("camera_trigger_secondary"); add_topic("cpuload"); - add_topic("distance_sensor", 100); add_topic("ekf2_innovations", 200); - //add_topic("ekf_gps_drift"); + add_topic("ekf_gps_drift"); add_topic("esc_status", 250); add_topic("estimator_status", 200); add_topic("home_position"); add_topic("input_rc", 200); add_topic("manual_control_setpoint", 200); - //add_topic("mission"); - //add_topic("mission_result"); + add_topic("mission"); + add_topic("mission_result"); add_topic("optical_flow", 50); + add_topic("position_controller_status", 500); add_topic("position_setpoint_triplet", 200); - //add_topic("radio_status"); - add_topic("rate_ctrl_status", 30); + add_topic("radio_status"); + add_topic("rate_ctrl_status", 200); add_topic("sensor_combined", 100); add_topic("sensor_preflight", 200); add_topic("system_power", 500); add_topic("tecs_status", 200); add_topic("trajectory_setpoint", 200); - add_topic("telemetry_status"); add_topic("vehicle_air_data", 200); - add_topic("vehicle_attitude", 30); + add_topic("vehicle_angular_velocity", 20); + add_topic("vehicle_attitude", 50); add_topic("vehicle_attitude_setpoint", 100); add_topic("vehicle_command"); add_topic("vehicle_global_position", 200); - add_topic("vehicle_gps_position"); add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_magnetometer", 200); - add_topic("vehicle_rates_setpoint", 30); + add_topic("vehicle_rates_setpoint", 20); add_topic("vehicle_status", 200); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); - add_topic("wind_estimate", 200); + + add_topic_multi("actuator_outputs", 100); + add_topic_multi("battery_status", 500); + add_topic_multi("distance_sensor", 100); + add_topic_multi("telemetry_status"); + add_topic_multi("vehicle_gps_position"); + add_topic_multi("wind_estimate", 200); #ifdef CONFIG_ARCH_BOARD_PX4_SITL + add_topic("actuator_controls_virtual_fw"); add_topic("actuator_controls_virtual_mc"); add_topic("fw_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint"); - add_topic("multirotor_motor_limits"); - add_topic("position_controller_status"); add_topic("offboard_control_mode"); + add_topic("position_controller_status"); add_topic("time_offset"); + add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_attitude_groundtruth", 10); add_topic("vehicle_global_position_groundtruth", 100); add_topic("vehicle_local_position_groundtruth", 100); add_topic("vehicle_roi"); + + add_topic_multi("multirotor_motor_limits"); + #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ } @@ -657,6 +585,7 @@ void Logger::add_high_rate_topics() add_topic("manual_control_setpoint"); add_topic("rate_ctrl_status"); add_topic("sensor_combined"); + add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); add_topic("vehicle_rates_setpoint"); @@ -664,10 +593,10 @@ void Logger::add_high_rate_topics() void Logger::add_debug_topics() { + add_topic("debug_array"); add_topic("debug_key_value"); add_topic("debug_value"); add_topic("debug_vect"); - add_topic("debug_array"); } void Logger::add_estimator_replay_topics() @@ -678,31 +607,32 @@ void Logger::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); - add_topic("distance_sensor"); add_topic("optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); add_topic("vehicle_air_data"); - add_topic("vehicle_gps_position"); add_topic("vehicle_land_detected"); add_topic("vehicle_magnetometer"); add_topic("vehicle_status"); add_topic("vehicle_visual_odometry"); + + add_topic_multi("distance_sensor"); + add_topic_multi("vehicle_gps_position"); } void Logger::add_thermal_calibration_topics() { - add_topic("sensor_accel", 100); - add_topic("sensor_baro", 100); - add_topic("sensor_gyro", 100); + add_topic_multi("sensor_accel", 100); + add_topic_multi("sensor_baro", 100); + add_topic_multi("sensor_gyro", 100); } void Logger::add_sensor_comparison_topics() { - add_topic("sensor_accel", 100); - add_topic("sensor_baro", 100); - add_topic("sensor_gyro", 100); - add_topic("sensor_mag", 100); + add_topic_multi("sensor_accel", 100); + add_topic_multi("sensor_baro", 100); + add_topic_multi("sensor_gyro", 100); + add_topic_multi("sensor_mag", 100); } void Logger::add_vision_and_avoidance_topics() @@ -725,14 +655,10 @@ void Logger::add_system_identification_topics() int Logger::add_topics_from_file(const char *fname) { - FILE *fp; - char line[80]; - char topic_name[80]; - unsigned interval; - int ntopics = 0; + int ntopics = 0; /* open the topic list file */ - fp = fopen(fname, "r"); + FILE *fp = fopen(fname, "r"); if (fp == nullptr) { return -1; @@ -740,8 +666,8 @@ int Logger::add_topics_from_file(const char *fname) /* call add_topic for each topic line in the file */ for (;;) { - /* get a line, bail on error/EOF */ + char line[80]; line[0] = '\0'; if (fgets(line, sizeof(line), fp) == nullptr) { @@ -754,8 +680,9 @@ int Logger::add_topics_from_file(const char *fname) } // read line with format: [, ] - interval = 0; - int nfields = sscanf(line, "%s %u", topic_name, &interval); + char topic_name[80]; + uint32_t interval_ms = 0; + int nfields = sscanf(line, "%s %u", topic_name, &interval_ms); if (nfields > 0) { int name_len = strlen(topic_name); @@ -764,8 +691,8 @@ int Logger::add_topics_from_file(const char *fname) topic_name[name_len - 1] = '\0'; } - /* add topic with specified interval */ - if (add_topic(topic_name, interval)) { + /* add topic with specified interval_ms */ + if (add_topic(topic_name, interval_ms)) { ntopics++; } else { @@ -854,15 +781,15 @@ void Logger::initialize_configured_topics() } -void Logger::add_mission_topic(const char *name, unsigned interval) +void Logger::add_mission_topic(const char *name, uint32_t interval_ms) { if (_num_mission_subs >= MAX_MISSION_TOPICS_NUM) { PX4_ERR("Max num mission topics exceeded"); return; } - if (add_topic(name, interval)) { - _mission_subscriptions[_num_mission_subs].min_delta_ms = interval; + if (add_topic(name, interval_ms)) { + _mission_subscriptions[_num_mission_subs].min_delta_ms = interval_ms; _mission_subscriptions[_num_mission_subs].next_write_time = 0; ++_num_mission_subs; } @@ -898,10 +825,7 @@ void Logger::run() } } - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - uORB::SubscriptionData parameter_update_sub(ORB_ID(parameter_update)); - int log_message_sub = orb_subscribe(ORB_ID(log_message)); - orb_set_interval(log_message_sub, 20); + uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); // mission log topics if enabled (must be added first) int32_t mission_log_mode = 0; @@ -919,12 +843,6 @@ void Logger::run() } } - int manual_control_sp_sub = -1; - - if (_log_mode == LogMode::rc_aux1) { - manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - } - int ntopics = add_topics_from_file(PX4_STORAGEDIR "/etc/logging/logger_topics.txt"); if (ntopics > 0) { @@ -934,21 +852,13 @@ void Logger::run() initialize_configured_topics(); } - int vehicle_command_sub = -1; - orb_advert_t vehicle_command_ack_pub = nullptr; - - if (_writer.backend() & LogWriter::BackendMavlink) { - vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); - } - //all topics added. Get required message buffer size int max_msg_size = 0; - int ret = 0; for (const auto &subscription : _subscriptions) { //use o_size, because that's what orb_copy will use - if (subscription.metadata->o_size > max_msg_size) { - max_msg_size = subscription.metadata->o_size; + if (subscription.get_topic()->o_size > max_msg_size) { + max_msg_size = subscription.get_topic()->o_size; } } @@ -1030,8 +940,7 @@ void Logger::run() while (!should_exit()) { // Start/stop logging (depending on logging mode, by default when arming/disarming) - const bool logging_started = start_stop_logging(vehicle_status_sub, manual_control_sp_sub, - (MissionLogType)mission_log_mode); + const bool logging_started = start_stop_logging((MissionLogType)mission_log_mode); if (logging_started) { #ifdef DBGPRINT @@ -1041,10 +950,7 @@ void Logger::run() } /* check for logging command from MAVLink (start/stop streaming) */ - if (vehicle_command_sub >= 0) { - handle_vehicle_command_update(vehicle_command_sub, vehicle_command_ack_pub); - } - + handle_vehicle_command_update(); if (timer_callback_data.watchdog_triggered) { timer_callback_data.watchdog_triggered = false; @@ -1068,11 +974,11 @@ void Logger::run() } } - bool data_written = false; - /* Check if parameters have changed */ if (!_should_stop_file_log) { // do not record param changes after disarming - if (parameter_update_sub.update()) { + parameter_update_s param_update; + + if (parameter_update_sub.update(¶m_update)) { write_changed_parameters(LogType::Full); } } @@ -1083,52 +989,45 @@ void Logger::run() int sub_idx = 0; for (LoggerSubscription &sub : _subscriptions) { - /* each message consists of a header followed by an orb data object - */ - size_t msg_size = sizeof(ulog_message_data_header_s) + sub.metadata->o_size_no_padding; - /* if this topic has been updated, copy the new data into the message buffer * and write a message to the log */ - for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { - if (copy_if_updated_multi(sub_idx, instance, _msg_buffer + sizeof(ulog_message_data_header_s), - sub_idx == next_subscribe_topic_index)) { + const bool try_to_subscribe = (sub_idx == next_subscribe_topic_index); - uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); - //write one byte after another (necessary because of alignment) - _msg_buffer[0] = (uint8_t)write_msg_size; - _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); - _msg_buffer[2] = static_cast(ULogMessageType::DATA); - uint16_t write_msg_id = sub.msg_ids[instance]; - _msg_buffer[3] = (uint8_t)write_msg_id; - _msg_buffer[4] = (uint8_t)(write_msg_id >> 8); + if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_header_s), try_to_subscribe)) { + // each message consists of a header followed by an orb data object + const size_t msg_size = sizeof(ulog_message_data_header_s) + sub.get_topic()->o_size_no_padding; + const uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); + const uint16_t write_msg_id = sub.msg_id; - //PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.metadata->o_name, sub.metadata->o_size, msg_size); + //write one byte after another (necessary because of alignment) + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::DATA); + _msg_buffer[3] = (uint8_t)write_msg_id; + _msg_buffer[4] = (uint8_t)(write_msg_id >> 8); - // full log - if (write_message(LogType::Full, _msg_buffer, msg_size)) { + // PX4_INFO("topic: %s, size = %zu, out_size = %zu", sub.get_topic()->o_name, sub.get_topic()->o_size, msg_size); + + // full log + if (write_message(LogType::Full, _msg_buffer, msg_size)) { #ifdef DBGPRINT - total_bytes += msg_size; + total_bytes += msg_size; #endif /* DBGPRINT */ + } - data_written = true; - } - - // mission log - if (sub_idx < _num_mission_subs) { - if (_writer.is_started(LogType::Mission)) { - if (_mission_subscriptions[sub_idx].next_write_time < (loop_time / 100000)) { - unsigned delta_time = _mission_subscriptions[sub_idx].min_delta_ms; - - if (delta_time > 0) { - _mission_subscriptions[sub_idx].next_write_time = (loop_time / 100000) + delta_time / 100; - } + // mission log + if (sub_idx < _num_mission_subs) { + if (_writer.is_started(LogType::Mission)) { + if (_mission_subscriptions[sub_idx].next_write_time < (loop_time / 100000)) { + unsigned delta_time = _mission_subscriptions[sub_idx].min_delta_ms; - if (write_message(LogType::Mission, _msg_buffer, msg_size)) { - data_written = true; - } + if (delta_time > 0) { + _mission_subscriptions[sub_idx].next_write_time = (loop_time / 100000) + delta_time / 100; } + + write_message(LogType::Mission, _msg_buffer, msg_size); } } } @@ -1137,13 +1036,10 @@ void Logger::run() ++sub_idx; } - //check for new logging message(s) - bool log_message_updated = false; - ret = orb_check(log_message_sub, &log_message_updated); + // check for new logging message(s) + log_message_s log_message; - if (ret == 0 && log_message_updated) { - log_message_s log_message; - orb_copy(ORB_ID(log_message), log_message_sub, &log_message); + if (_log_message_sub.update(&log_message)) { const char *message = (const char *)log_message.text; int message_len = strlen(message); @@ -1184,7 +1080,7 @@ void Logger::run() // update buffer statistics for (int i = 0; i < (int)LogType::Count; ++i) { - if (!_statistics[i].dropout_start && _writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water) { + if (!_statistics[i].dropout_start && (_writer.get_buffer_fill_count_file((LogType)i) > _statistics[i].high_water)) { _statistics[i].high_water = _writer.get_buffer_fill_count_file((LogType)i); } } @@ -1192,10 +1088,8 @@ void Logger::run() /* release the log buffer */ _writer.unlock(); - /* notify the writer thread if data is available */ - if (data_written) { - _writer.notify(); - } + /* notify the writer thread */ + _writer.notify(); /* subscription update */ if (next_subscribe_topic_index != -1) { @@ -1216,10 +1110,8 @@ void Logger::run() // - we avoid subscribing to many topics at once, when logging starts // - we'll get the data immediately once we start logging (no need to wait for the next subscribe timeout) if (next_subscribe_topic_index != -1) { - for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { - if (_subscriptions[next_subscribe_topic_index].fd[instance] < 0) { - try_to_subscribe_topic(_subscriptions[next_subscribe_topic_index], instance); - } + if (!_subscriptions[next_subscribe_topic_index].valid()) { + _subscriptions[next_subscribe_topic_index].subscribe(); } if (++next_subscribe_topic_index >= (int)_subscriptions.size()) { @@ -1270,20 +1162,6 @@ void Logger::run() // stop the writer thread _writer.thread_stop(); - //unsubscribe - for (LoggerSubscription &sub : _subscriptions) { - for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; instance++) { - if (sub.fd[instance] >= 0) { - orb_unsubscribe(sub.fd[instance]); - sub.fd[instance] = -1; - } - } - } - - if (manual_control_sp_sub != -1) { - orb_unsubscribe(manual_control_sp_sub); - } - if (polling_topic_sub >= 0) { orb_unsubscribe(polling_topic_sub); } @@ -1293,14 +1171,6 @@ void Logger::run() _mavlink_log_pub = nullptr; } - if (vehicle_command_ack_pub) { - orb_unadvertise(vehicle_command_ack_pub); - } - - if (vehicle_command_sub != -1) { - orb_unsubscribe(vehicle_command_sub); - } - px4_unregister_shutdown_hook(&Logger::request_stop_static); } @@ -1325,21 +1195,20 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) #endif /* DBGPRINT */ } -bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type) +bool Logger::start_stop_logging(MissionLogType mission_log_type) { bool bret = false; bool want_start = false; bool want_stop = false; if (_log_mode == LogMode::rc_aux1) { - //aux1-based logging - bool manual_control_setpoint_updated; - int ret = orb_check(manual_control_sp_sub, &manual_control_setpoint_updated); - if (ret == 0 && manual_control_setpoint_updated) { - manual_control_setpoint_s manual_sp; - orb_copy(ORB_ID(manual_control_setpoint), manual_control_sp_sub, &manual_sp); - bool should_start = manual_sp.aux1 > 0.3f || _manually_logging_override; + // aux1-based logging + manual_control_setpoint_s manual_sp; + + if (_manual_control_sp_sub.update(&manual_sp)) { + + bool should_start = ((manual_sp.aux1 > 0.3f) || _manually_logging_override); if (_prev_state != should_start) { _prev_state = should_start; @@ -1355,12 +1224,10 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su } else { // arming-based logging - bool vehicle_status_updated; - int ret = orb_check(vehicle_status_sub, &vehicle_status_updated); + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { - if (ret == 0 && vehicle_status_updated) { - vehicle_status_s vehicle_status; - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || _manually_logging_override; if (_prev_state != armed && _log_mode != LogMode::boot_until_shutdown) { @@ -1402,34 +1269,28 @@ bool Logger::start_stop_logging(int vehicle_status_sub, int manual_control_sp_su return bret; } -void Logger::handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub) +void Logger::handle_vehicle_command_update() { - bool command_updated = false; - int ret = orb_check(vehicle_command_sub, &command_updated); + vehicle_command_s command; - if (ret == 0 && command_updated) { - vehicle_command_s command; - orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &command); + if (_vehicle_command_sub.update(&command)) { if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + if ((int)(command.param1 + 0.5f) != 0) { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); } else if (can_start_mavlink_log()) { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); start_log_mavlink(); } else { - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } } else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) { stop_log_mavlink(); - ack_vehicle_command(vehicle_command_ack_pub, &command, - vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } } } @@ -1832,6 +1693,13 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats return; } + // check if we already wrote the format + for (const auto &written_format : written_formats) { + if (written_format == &meta) { + return; + } + } + // Write the current format (we don't need to check if we already added it to written_formats) int format_len = snprintf(msg.format, sizeof(msg.format), "%s:%s", meta.o_name, meta.o_fields); size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len; @@ -1903,17 +1771,8 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats } if (found_topic) { - // check if we already wrote the format - for (const auto &written_format : written_formats) { - if (written_format == found_topic) { - found_topic = nullptr; - break; - } - } - if (found_topic) { - write_format(type, *found_topic, written_formats, msg, level + 1); - } + write_format(type, *found_topic, written_formats, msg, level + 1); } else { PX4_ERR("No definition for topic %s found", fmt); @@ -1943,7 +1802,7 @@ void Logger::write_formats(LogType type) for (size_t i = 0; i < sub_count; ++i) { const LoggerSubscription &sub = _subscriptions[i]; - write_format(type, *sub.metadata, written_formats, msg); + write_format(type, *sub.get_topic(), written_formats, msg); } _writer.unlock(); @@ -1962,36 +1821,34 @@ void Logger::write_all_add_logged_msg(LogType type) for (size_t i = 0; i < sub_count; ++i) { LoggerSubscription &sub = _subscriptions[i]; - for (int instance = 0; instance < ORB_MULTI_MAX_INSTANCES; ++instance) { - if (sub.fd[instance] >= 0) { - write_add_logged_msg(type, sub, instance); - } + if (sub.valid()) { + write_add_logged_msg(type, sub); } } _writer.unlock(); } -void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription, int instance) +void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription) { ulog_message_add_logged_s msg; - if (subscription.msg_ids[instance] == (uint8_t) - 1) { - if (_next_topic_id == (uint8_t) - 1) { + if (subscription.msg_id == MSG_ID_INVALID) { + if (_next_topic_id == MSG_ID_INVALID) { // if we land here an uint8 is too small -> switch to uint16 PX4_ERR("limit for _next_topic_id reached"); return; } - subscription.msg_ids[instance] = _next_topic_id++; + subscription.msg_id = _next_topic_id++; } - msg.msg_id = subscription.msg_ids[instance]; - msg.multi_id = instance; + msg.msg_id = subscription.msg_id; + msg.multi_id = subscription.get_instance(); - int message_name_len = strlen(subscription.metadata->o_name); + int message_name_len = strlen(subscription.get_topic()->o_name); - memcpy(msg.message_name, subscription.metadata->o_name, message_name_len); + memcpy(msg.message_name, subscription.get_topic()->o_name, message_name_len); size_t msg_size = sizeof(msg) - sizeof(msg.message_name) + message_name_len; msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; @@ -2339,7 +2196,7 @@ void Logger::write_changed_parameters(LogType type) _writer.notify(); } -void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result) +void Logger::ack_vehicle_command(vehicle_command_s *cmd, uint32_t result) { vehicle_command_ack_s vehicle_command_ack = {}; vehicle_command_ack.timestamp = hrt_absolute_time(); @@ -2348,13 +2205,8 @@ void Logger::ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_ vehicle_command_ack.target_system = cmd->source_system; vehicle_command_ack.target_component = cmd->source_component; - if (vehicle_command_ack_pub == nullptr) { - vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), vehicle_command_ack_pub, &vehicle_command_ack); - } + uORB::PublicationQueued cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); } int Logger::print_usage(const char *reason) @@ -2407,7 +2259,6 @@ Or if already running: PRINT_MODULE_USAGE_PARAM_FLAG('t', "Use date/time for naming log directories and files", true); PRINT_MODULE_USAGE_PARAM_INT('r', 280, 0, 8000, "Log rate in Hz, 0 means unlimited rate", true); PRINT_MODULE_USAGE_PARAM_INT('b', 12, 4, 10000, "Log buffer size in KiB", true); - PRINT_MODULE_USAGE_PARAM_INT('q', 14, 1, 100, "uORB queue size for mavlink mode", true); PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "", "Poll on a topic instead of running with fixed rate (Log rate and topic intervals are ignored if this is set)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("on", "start logging now, override arming (logger must be running)"); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index e76146bc1dbf..fbecac47af24 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -39,17 +39,21 @@ #include "util.h" #include #include -#include #include #include #include #include +#include +#include +#include +#include #include +#include extern "C" __EXPORT int logger_main(int argc, char *argv[]); -#define TRY_SUBSCRIBE_INTERVAL 1000*1000 // interval in microseconds at which we try to subscribe to a topic +static constexpr hrt_abstime TRY_SUBSCRIBE_INTERVAL{1000 * 1000}; // interval in microseconds at which we try to subscribe to a topic // if we haven't succeeded before namespace px4 @@ -79,27 +83,17 @@ inline bool operator&(SDLogProfileMask a, SDLogProfileMask b) return static_cast(a) & static_cast(b); } -struct LoggerSubscription { - int fd[ORB_MULTI_MAX_INSTANCES]; ///< uorb subscription. The first fd is also used to store the interval if - /// not subscribed yet (-interval - 1) - const orb_metadata *metadata = nullptr; - uint8_t msg_ids[ORB_MULTI_MAX_INSTANCES]; +static constexpr uint8_t MSG_ID_INVALID = UINT8_MAX; - LoggerSubscription() {} +struct LoggerSubscription : public uORB::SubscriptionInterval { - LoggerSubscription(int fd_, const orb_metadata *metadata_) : - metadata(metadata_) - { - fd[0] = fd_; + uint8_t msg_id{MSG_ID_INVALID}; - for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { - fd[i] = -1; - } + LoggerSubscription() = default; - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - msg_ids[i] = (uint8_t) - 1; - } - } + LoggerSubscription(const orb_metadata *meta, uint32_t interval_ms = 0, uint8_t instance = 0) : + uORB::SubscriptionInterval(meta, interval_ms * 1000, instance) + {} }; class Logger : public ModuleBase @@ -113,7 +107,7 @@ class Logger : public ModuleBase }; Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_interval, const char *poll_topic_name, - LogMode log_mode, bool log_name_timestamp, unsigned int queue_size); + LogMode log_mode, bool log_name_timestamp); ~Logger(); @@ -146,18 +140,19 @@ class Logger : public ModuleBase * Add a topic to be logged. This must be called before start_log() * (because it does not write an ADD_LOGGED_MSG message). * @param name topic name - * @param interval limit rate if >0, otherwise log as fast as the topic is updated. + * @param interval limit in milliseconds if >0, otherwise log as fast as the topic is updated. + * @param instance orb topic instance * @return true on success */ - bool add_topic(const char *name, unsigned interval = 0); + bool add_topic(const char *name, uint32_t interval_ms = 0, uint8_t instance = 0); + bool add_topic_multi(const char *name, uint32_t interval_ms = 0); /** * add a logged topic (called by add_topic() above). * In addition, it subscribes to the first instance of the topic, if it's advertised, - * and sets the file descriptor of LoggerSubscription accordingly * @return the newly added subscription on success, nullptr otherwise */ - LoggerSubscription *add_topic(const orb_metadata *topic); + LoggerSubscription *add_topic(const orb_metadata *topic, uint32_t interval_ms = 0, uint8_t instance = 0); /** * request the logger thread to stop (this method does not block). @@ -177,7 +172,7 @@ class Logger : public ModuleBase Watchdog }; - static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */ + static constexpr size_t MAX_TOPICS_NUM = 90; /**< Maximum number of logged topics */ static constexpr int MAX_MISSION_TOPICS_NUM = 5; /**< Maximum number of mission topics */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static constexpr const char *LOG_ROOT[(int)LogType::Count] = { @@ -214,7 +209,7 @@ class Logger : public ModuleBase * Write an ADD_LOGGED_MSG to the log for a given subscription and instance. * _writer.lock() must be held when calling this. */ - void write_add_logged_msg(LogType type, LoggerSubscription &subscription, int instance); + void write_add_logged_msg(LogType type, LoggerSubscription &subscription); /** * Create logging directory @@ -297,13 +292,7 @@ class Logger : public ModuleBase void write_changed_parameters(LogType type); - inline bool copy_if_updated_multi(int sub_idx, int multi_instance, void *buffer, bool try_to_subscribe); - - /** - * Check if a topic instance exists and subscribe to it - * @return true when topic exists and subscription successful - */ - bool try_to_subscribe_topic(LoggerSubscription &sub, int multi_instance); + inline bool copy_if_updated(int sub_idx, void *buffer, bool try_to_subscribe); /** * Write exactly one ulog message to the logger and handle dropouts. @@ -331,7 +320,7 @@ class Logger : public ModuleBase * @param name topic name * @param interval limit rate if >0 [ms], otherwise log as fast as the topic is updated. */ - void add_mission_topic(const char *name, unsigned interval = 0); + void add_mission_topic(const char *name, uint32_t interval_ms = 0); /** * Add topic subscriptions based on the _sdlog_profile_handle parameter @@ -355,10 +344,10 @@ class Logger : public ModuleBase * @param mission_log_type * @return true if log started */ - bool start_stop_logging(int vehicle_status_sub, int manual_control_sp_sub, MissionLogType mission_log_type); + bool start_stop_logging(MissionLogType mission_log_type); - void handle_vehicle_command_update(int vehicle_command_sub, orb_advert_t &vehicle_command_ack_pub); - void ack_vehicle_command(orb_advert_t &vehicle_command_ack_pub, vehicle_command_s *cmd, uint32_t result); + void handle_vehicle_command_update(); + void ack_vehicle_command(vehicle_command_s *cmd, uint32_t result); /** * initialize the output for the process load, so that ~1 second later it will be written to the log @@ -408,6 +397,11 @@ class Logger : public ModuleBase hrt_abstime _next_load_print{0}; ///< timestamp when to print the process load PrintLoadReason _print_load_reason {PrintLoadReason::Preflight}; + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20}; + param_t _sdlog_profile_handle{PARAM_INVALID}; param_t _log_utc_offset{PARAM_INVALID}; param_t _log_dirs_max{PARAM_INVALID}; diff --git a/src/modules/logger/util.cpp b/src/modules/logger/util.cpp index 958369c062d6..832718593d82 100644 --- a/src/modules/logger/util.cpp +++ b/src/modules/logger/util.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -72,18 +73,15 @@ bool file_exist(const char *filename) bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) { - int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; - if (vehicle_gps_position_sub < 0) { - return false; - } + time_t utc_time_sec; + bool use_clock_time = true; /* Get the latest GPS publication */ vehicle_gps_position_s gps_pos; - time_t utc_time_sec; - bool use_clock_time = true; - if (orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps_pos) == 0) { + if (vehicle_gps_position_sub.copy(&gps_pos)) { utc_time_sec = gps_pos.time_utc_usec / 1e6; if (gps_pos.fix_type >= 2 && utc_time_sec >= GPS_EPOCH_SECS) { @@ -91,8 +89,6 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) } } - orb_unsubscribe(vehicle_gps_position_sub); - if (use_clock_time) { /* take clock time if there's no fix (yet) */ struct timespec ts = {}; diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 201d98ebf48f..0df7d2ff4925 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -36,8 +36,6 @@ px4_add_git_submodule(TARGET git_mavlink_v2 PATH "${PX4_SOURCE_DIR}/mavlink/incl px4_add_module( MODULE modules__mavlink MAIN mavlink - STACK_MAIN 1600 - STACK_MAX 1600 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable -Wno-address-of-packed-member # TODO: fix in c_library_v2 diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 11e34248b837..841de386a1dd 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -112,7 +112,7 @@ int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s } void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_t &ack, - uint8_t from_sysid, uint8_t from_compid) + uint8_t from_sysid, uint8_t from_compid, uint8_t channel) { CMD_DEBUG("handling result %d for command %d (from %d:%d)", ack.result, ack.command, from_sysid, from_compid); @@ -123,11 +123,10 @@ void MavlinkCommandSender::handle_mavlink_command_ack(const mavlink_command_ack_ while (command_item_t *item = _commands.get_next()) { // Check if the incoming ack matches any of the commands that we have sent. if (item->command.command == ack.command && - from_sysid == item->command.target_system && - from_compid == item->command.target_component) { - // Drop it anyway because the command seems to have arrived at the destination, even if we - // receive IN_PROGRESS because we trust that it will be handled after that. - _commands.drop_current(); + (item->command.target_system == 0 || from_sysid == item->command.target_system) && + (item->command.target_component == 0 || from_compid == item->command.target_component) && + item->num_sent_per_channel[channel] != -1) { + item->num_sent_per_channel[channel] = -2; // mark this as acknowledged break; } } @@ -147,6 +146,23 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) continue; } + // Loop through num_sent_per_channel and check if any channel has receives an ack for this command + // (indicated by the value -2). We avoid removing the command at the time of receiving the ack + // as some channels might be lagging behind and will end up putting the same command into the buffer. + bool dropped_command = false; + + for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + if (item->num_sent_per_channel[i] == -2) { + _commands.drop_current(); + dropped_command = true; + break; + } + } + + if (dropped_command) { + continue; + } + // The goal of this is to retry from all channels. Therefore, we keep // track of the retry count for each channel. // @@ -171,7 +187,7 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) } } - if (item->num_sent_per_channel[channel] < max_sent) { + if (item->num_sent_per_channel[channel] < max_sent && item->num_sent_per_channel[channel] != -1) { // We are behind and need to do a retransmission. mavlink_msg_command_long_send_struct(channel, &item->command); item->num_sent_per_channel[channel]++; diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h index 5e2d17493069..ba36af11559d 100644 --- a/src/modules/mavlink/mavlink_command_sender.h +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -81,7 +81,8 @@ class MavlinkCommandSender * Handle mavlink command_ack. * thread-safe */ - void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid); + void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid, + uint8_t channel); private: MavlinkCommandSender() = default; @@ -108,7 +109,7 @@ class MavlinkCommandSender mavlink_command_long_t command = {}; hrt_abstime timestamp_us = 0; hrt_abstime last_time_sent_us = 0; - int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; + int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command } command_item_t; TimestampedList _commands{3}; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a1042d078331..5831c45b4a4a 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -138,14 +138,17 @@ MavlinkFTP::handle_message(const mavlink_message_t *msg) if ((ftp_request.target_system == _getServerSystemId() || ftp_request.target_system == 0) && (ftp_request.target_component == _getServerComponentId() || ftp_request.target_component == 0)) { - _process_request(&ftp_request, msg->sysid); + _process_request(&ftp_request, msg->sysid, msg->compid); } } } /// @brief Processes an FTP message void -MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id) +MavlinkFTP::_process_request( + mavlink_file_transfer_protocol_t *ftp_req, + uint8_t target_system_id, + uint8_t target_comp_id) { bool stream_send = false; PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); @@ -218,7 +221,7 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t break; case kCmdBurstReadFile: - errorCode = _workBurst(payload, target_system_id); + errorCode = _workBurst(payload, target_system_id, target_comp_id); stream_send = true; break; @@ -287,6 +290,8 @@ MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t if (!stream_send || errorCode != kErrNone) { // respond to the request ftp_req->target_system = target_system_id; + ftp_req->target_network = 0; + ftp_req->target_component = target_comp_id; _reply(ftp_req); } } @@ -310,7 +315,6 @@ bool MavlinkFTP::_ensure_buffers_exist() void MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) { - PayloadHeader *payload = reinterpret_cast(&ftp_req->payload[0]); // keep a copy of the last sent response ((n)ack), so that if it gets lost and the GCS resends the request, @@ -326,8 +330,6 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) PX4_INFO("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number); #endif - ftp_req->target_network = 0; - ftp_req->target_component = 0; #ifdef MAVLINK_FTP_UNIT_TEST // Unit test hook is set, call that instead _utRcvMsgFunc(ftp_req, _worker_data); @@ -575,7 +577,7 @@ MavlinkFTP::_workRead(PayloadHeader *payload) /// @brief Responds to a Stream command MavlinkFTP::ErrorCode -MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id) +MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id) { if (payload->session != 0 && _session_info.fd < 0) { return kErrInvalidSession; @@ -590,6 +592,7 @@ MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id) _session_info.stream_chunk_transmitted = 0; _session_info.stream_seq_number = payload->seq_number + 1; _session_info.stream_target_system_id = target_system_id; + _session_info.stream_target_component_id = target_component_id; return kErrNone; } @@ -1079,6 +1082,8 @@ void MavlinkFTP::send(const hrt_abstime t) } ftp_msg.target_system = _session_info.stream_target_system_id; + ftp_msg.target_network = 0; + ftp_msg.target_component = _session_info.stream_target_component_id; _reply(&ftp_msg); } while (more_data); } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index f178d3f8214e..a94f61cd214e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -128,14 +128,14 @@ class MavlinkFTP private: char *_data_as_cstring(PayloadHeader *payload); - void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id); + void _process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id, uint8_t target_comp_id); void _reply(mavlink_file_transfer_protocol_t *ftp_req); int _copy_file(const char *src_path, const char *dst_path, size_t length); ErrorCode _workList(PayloadHeader *payload, bool list_hidden = false); ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); - ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id); + ErrorCode _workBurst(PayloadHeader *payload, uint8_t target_system_id, uint8_t target_component_id); ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload); ErrorCode _workReset(PayloadHeader *payload); @@ -170,6 +170,7 @@ class MavlinkFTP uint32_t stream_offset; uint16_t stream_seq_number; uint8_t stream_target_system_id; + uint8_t stream_target_component_id; unsigned stream_chunk_transmitted; }; struct SessionInfo _session_info {}; ///< Session info, fd=-1 for no active session diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index ac2859f1d2a5..7723f653b9b9 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -101,7 +101,7 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _windspeed(SimpleAnalyzer::AVERAGE) { - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); } @@ -119,7 +119,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) bool updated = _airspeed.valid(); updated |= _airspeed_sp.valid(); - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { updated |= _batteries[i].analyzer.valid(); } @@ -159,7 +159,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t) int lowest = 0; - for (int i = 1; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 1; i < ORB_MULTI_MAX_INSTANCES; i++) { const bool battery_connected = _batteries[i].connected && _batteries[i].analyzer.valid(); const bool battery_is_lowest = _batteries[i].analyzer.get_scaled(100.0f) <= _batteries[lowest].analyzer.get_scaled( 100.0f); @@ -230,7 +230,7 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t) _airspeed.reset(); _airspeed_sp.reset(); - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _batteries[i].analyzer.reset(); } @@ -278,7 +278,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms struct battery_status_s battery; bool updated = false; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -535,7 +535,7 @@ void MavlinkStreamHighLatency2::update_battery_status() { battery_status_s battery; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (_batteries[i].subscription->update(&battery)) { _batteries[i].connected = battery.connected; _batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered); diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 9de3be2f9b22..bf9d63c44b75 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -148,7 +148,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream hrt_abstime _last_update_time = 0; float _update_rate_filtered = 0.0f; - PerBatteryData _batteries[BOARD_NUMBER_BRICKS]; + PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES]; /* do not allow top copying this class */ MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 946aa8cce446..a78fc243a2f3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -49,8 +49,9 @@ #endif #include -#include -#include +#include +#include +#include #include "mavlink_receiver.h" #include "mavlink_main.h" @@ -163,50 +164,6 @@ bool Mavlink::_boot_complete = false; Mavlink::Mavlink() : ModuleParams(nullptr) { - _instance_id = Mavlink::instance_count(); - - /* set channel according to instance id */ - switch (_instance_id) { - case 0: - _channel = MAVLINK_COMM_0; - break; - - case 1: - _channel = MAVLINK_COMM_1; - break; - - case 2: - _channel = MAVLINK_COMM_2; - break; - - case 3: - _channel = MAVLINK_COMM_3; - break; -#ifdef MAVLINK_COMM_4 - - case 4: - _channel = MAVLINK_COMM_4; - break; -#endif -#ifdef MAVLINK_COMM_5 - - case 5: - _channel = MAVLINK_COMM_5; - break; -#endif -#ifdef MAVLINK_COMM_6 - - case 6: - _channel = MAVLINK_COMM_6; - break; -#endif - - default: - PX4_WARN("instance ID is out of range"); - px4_task_exit(1); - break; - } - // initialise parameter cache mavlink_update_parameters(); @@ -269,6 +226,58 @@ Mavlink::mavlink_update_parameters() } } +void +Mavlink::set_channel() +{ + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + + case 1: + _channel = MAVLINK_COMM_1; + break; + + case 2: + _channel = MAVLINK_COMM_2; + break; + + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + + default: + PX4_WARN("instance ID is out of range"); + px4_task_exit(1); + break; + } +} + +void +Mavlink::set_instance_id() +{ + _instance_id = Mavlink::instance_count(); +} + void Mavlink::set_proto_version(unsigned version) { @@ -410,14 +419,14 @@ Mavlink::get_status_all_instances(bool show_streams_status) } bool -Mavlink::instance_exists(const char *device_name, Mavlink *self) +Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) { Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { - /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->_device_name)) { + /* don't compare with itself and with non serial instances*/ + if ((inst != self) && (inst->get_protocol() == SERIAL) && !strcmp(device_name, inst->_device_name)) { return true; } @@ -441,12 +450,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) // might be nullptr if message is unknown if (meta) { // Extract target system and target component if set - if (meta->target_system_ofs != 0) { - target_system_id = ((uint8_t *)msg)[meta->target_system_ofs]; + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) { + target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs]; } - if (meta->target_component_ofs != 0) { - target_component_id = ((uint8_t *)msg)[meta->target_component_ofs]; + if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) { + target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs]; } } @@ -485,7 +494,7 @@ Mavlink::get_uart_fd(unsigned index) } int -Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_control) +Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const bool force_flow_control) { #ifndef B460800 #define B460800 460800 @@ -603,7 +612,9 @@ Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_cont return -1; } - px4_usleep(100000); + int errcode = errno; + /* ENOTCONN means that the USB device is not yet connected */ + px4_usleep(errcode == ENOTCONN ? 1000000 : 100000); _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); } } @@ -1620,6 +1631,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("LOCAL_POSITION_NED", 1.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); + configure_stream_local("OBSTACLE_DISTANCE", 5.0f); configure_stream_local("ODOMETRY", 3.0f); configure_stream_local("OPTICAL_FLOW_RAD", 1.0f); configure_stream_local("ORBIT_EXECUTION_STATUS", 5.0f); @@ -2063,7 +2075,7 @@ Mavlink::task_main(int argc, char *argv[]) } if (get_protocol() == SERIAL) { - if (Mavlink::instance_exists(_device_name, this)) { + if (Mavlink::serial_instance_exists(_device_name, this)) { PX4_ERR("%s already running", _device_name); return PX4_ERROR; } @@ -2126,7 +2138,7 @@ Mavlink::task_main(int argc, char *argv[]) cmd_sub->subscribe_from_beginning(true); /* command ack */ - orb_advert_t command_ack_pub = nullptr; + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log)); @@ -2172,6 +2184,10 @@ Mavlink::task_main(int argc, char *argv[]) _main_loop_delay = MAVLINK_MAX_INTERVAL; } + set_instance_id(); + + set_channel(); + /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); @@ -2263,7 +2279,7 @@ Mavlink::task_main(int argc, char *argv[]) } // send positive command ack - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = vehicle_cmd.timestamp; command_ack.command = vehicle_cmd.command; command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; @@ -2271,13 +2287,7 @@ Mavlink::task_main(int argc, char *argv[]) command_ack.target_system = vehicle_cmd.source_system; command_ack.target_component = vehicle_cmd.source_component; - if (command_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); - - } else { - command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + command_ack_pub.publish(command_ack); } } @@ -2542,12 +2552,7 @@ void Mavlink::publish_telemetry_status() _tstatus.timestamp = hrt_absolute_time(); - if (_telem_status_pub == nullptr) { - _telem_status_pub = orb_advertise_queue(ORB_ID(telemetry_status), &_tstatus, 3); - - } else { - orb_publish(ORB_ID(telemetry_status), _telem_status_pub, &_tstatus); - } + _telem_status_pub.publish(_tstatus); } void Mavlink::check_radio_config() diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5ab8d0fb9b7e..e222846ff311 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -71,11 +71,11 @@ #include #include #include +#include #include #include #include #include -#include #include "mavlink_command_sender.h" #include "mavlink_messages.h" @@ -83,6 +83,7 @@ #include "mavlink_shell.h" #include "mavlink_ulog.h" +#define DEFAULT_BAUD_RATE 57600 #define DEFAULT_REMOTE_PORT_UDP 14550 ///< GCS port per MAVLink spec #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define HASH_PARAM "_HASH_CHECK" @@ -155,7 +156,7 @@ class Mavlink : public ModuleParams static int get_status_all_instances(bool show_streams_status); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool serial_instance_exists(const char *device_name, Mavlink *self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -540,7 +541,8 @@ class Mavlink : public ModuleParams bool _first_heartbeat_sent{false}; orb_advert_t _mavlink_log_pub{nullptr}; - orb_advert_t _telem_status_pub{nullptr}; + + uORB::PublicationQueued _telem_status_pub{ORB_ID(telemetry_status)}; bool _task_running{false}; static bool _boot_complete; @@ -675,7 +677,9 @@ class Mavlink : public ModuleParams void mavlink_update_parameters(); - int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control); + int mavlink_open_uart(const int baudrate = DEFAULT_BAUD_RATE, + const char *uart_name = DEFAULT_DEVICE_NAME, + const bool force_flow_control = false); static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25; static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35; @@ -730,10 +734,14 @@ class Mavlink : public ModuleParams void init_udp(); + void set_channel(); + + void set_instance_id(); + /** * Main mavlink task. */ - int task_main(int argc, char *argv[]); + int task_main(int argc, char *argv[]); // Disallow copy construction and move assignment. Mavlink(const Mavlink &) = delete; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 64555ec53132..6e97675f39be 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include @@ -84,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -291,7 +293,7 @@ void get_mavlink_mode_state(const struct vehicle_status_s *const status, uint8_t } else if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == vehicle_status_s::ARMING_STATE_REBOOT) { + } else if (status->arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) { *mavlink_state = MAV_STATE_POWEROFF; } else { @@ -542,11 +544,11 @@ class MavlinkStreamSysStatus : public MavlinkStream private: MavlinkOrbSubscription *_status_sub; MavlinkOrbSubscription *_cpuload_sub; - MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS]; + MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES]; uint64_t _status_timestamp{0}; uint64_t _cpuload_timestamp{0}; - uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS]; + uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {}; /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &) = delete; @@ -557,7 +559,7 @@ class MavlinkStreamSysStatus : public MavlinkStream _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _cpuload_sub(_mavlink->add_orb_subscription(ORB_ID(cpuload))) { - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); _battery_status_timestamp[i] = 0; } @@ -565,63 +567,55 @@ class MavlinkStreamSysStatus : public MavlinkStream bool send(const hrt_abstime t) { - vehicle_status_s status = {}; - cpuload_s cpuload = {}; - battery_status_s battery_status[BOARD_NUMBER_BRICKS] = {}; + vehicle_status_s status{}; + cpuload_s cpuload{}; + battery_status_s battery_status[ORB_MULTI_MAX_INSTANCES] {}; const bool updated_status = _status_sub->update(&_status_timestamp, &status); const bool updated_cpuload = _cpuload_sub->update(&_cpuload_timestamp, &cpuload); - bool updated_any = updated_status || updated_cpuload; - bool updated_battery[BOARD_NUMBER_BRICKS]; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { - updated_battery[i] = _battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i]); - updated_any = updated_any || updated_battery[i]; - } - - if (updated_any) { + bool updated_battery = false; - if (!updated_status) { - _status_sub->update(&status); + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status[i])) { + updated_battery = true; } + } - battery_status_s *lowest_battery = &battery_status[0]; - - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { - if (!updated_battery[i]) { - _battery_status_sub[i]->update(&battery_status[i]); - } + if (updated_status || updated_cpuload || updated_battery) { + int lowest_battery_index = 0; - if (battery_status[i].connected && battery_status[i].remaining < lowest_battery->remaining) { - lowest_battery = &battery_status[i]; + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (battery_status[i].connected && (battery_status[i].remaining < battery_status[lowest_battery_index].remaining)) { + lowest_battery_index = i; } } - if (!updated_cpuload) { - _cpuload_sub->update(&cpuload); - } - - mavlink_sys_status_t msg = {}; + mavlink_sys_status_t msg{}; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; msg.onboard_control_sensors_health = status.onboard_control_sensors_health; + msg.load = cpuload.load * 1000.0f; + // TODO: Determine what data should be put here when there are multiple batteries. // Right now, it uses the lowest battery. This is a safety decision, because if a client is only checking // one battery using this message, it should be the lowest. // In the future, this should somehow determine the "main" battery, or use the "type" field of BATTERY_STATUS // to determine which battery is more important at a given time. - msg.voltage_battery = (lowest_battery->connected) ? lowest_battery->voltage_filtered_v * 1000.0f : UINT16_MAX; - msg.current_battery = (lowest_battery->connected) ? lowest_battery->current_filtered_a * 100.0f : -1; - msg.battery_remaining = (lowest_battery->connected) ? ceilf(lowest_battery->remaining * 100.0f) : -1; - // TODO: fill in something useful in the fields below - msg.drop_rate_comm = 0; - msg.errors_comm = 0; - msg.errors_count1 = 0; - msg.errors_count2 = 0; - msg.errors_count3 = 0; - msg.errors_count4 = 0; + const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; + + if (lowest_battery.connected) { + msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; + msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); + + } else { + msg.voltage_battery = UINT16_MAX; + msg.current_battery = -1; + msg.battery_remaining = -1; + } mavlink_msg_sys_status_send_struct(_mavlink->get_channel(), &msg); @@ -666,9 +660,9 @@ class MavlinkStreamBatteryStatus : public MavlinkStream } private: - MavlinkOrbSubscription *_battery_status_sub[BOARD_NUMBER_BRICKS] {}; + MavlinkOrbSubscription *_battery_status_sub[ORB_MULTI_MAX_INSTANCES] {}; - uint64_t _battery_status_timestamp[BOARD_NUMBER_BRICKS] {}; + uint64_t _battery_status_timestamp[ORB_MULTI_MAX_INSTANCES] {}; /* do not allow top copying this class */ MavlinkStreamBatteryStatus(MavlinkStreamSysStatus &) = delete; @@ -677,7 +671,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream protected: explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) { - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { _battery_status_sub[i] = _mavlink->add_orb_subscription(ORB_ID(battery_status), i); } } @@ -686,13 +680,13 @@ class MavlinkStreamBatteryStatus : public MavlinkStream { bool updated = false; - for (int i = 0; i < BOARD_NUMBER_BRICKS; i++) { + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (!_battery_status_sub[i]) { continue; } - battery_status_s battery_status; + battery_status_s battery_status{}; if (_battery_status_sub[i]->update(&_battery_status_timestamp[i], &battery_status)) { /* battery status message with higher resolution */ @@ -704,16 +698,19 @@ class MavlinkStreamBatteryStatus : public MavlinkStream bat_msg.energy_consumed = -1; bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? ceilf(battery_status.remaining * 100.0f) : -1; - bat_msg.temperature = (battery_status.connected) ? (int16_t)battery_status.temperature : INT16_MAX; - //bat_msg.average_current_battery = (battery_status.connected) ? battery_status.average_current_a * 100.0f : -1; - //bat_msg.serial_number = (battery_status.connected) ? battery_status.serial_number : 0; - //bat_msg.capacity = (battery_status.connected) ? battery_status.capacity : 0; - //bat_msg.cycle_count = (battery_status.connected) ? battery_status.cycle_count : UINT16_MAX; - //bat_msg.run_time_to_empty = (battery_status.connected) ? battery_status.run_time_to_empty * 60 : 0; - //bat_msg.average_time_to_empty = (battery_status.connected) ? battery_status.average_time_to_empty * 60 : 0; - - for (unsigned cell = 0; cell < (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); cell++) { - if ((int32_t)cell < battery_status.cell_count && battery_status.connected) { + + // check if temperature valid + if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) { + bat_msg.temperature = battery_status.temperature * 100.0f; + + } else { + bat_msg.temperature = INT16_MAX; + } + + static constexpr int mavlink_cells_max = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); + + for (int cell = 0; cell < mavlink_cells_max; cell++) { + if ((battery_status.cell_count > 0) && (cell < battery_status.cell_count) && battery_status.connected) { bat_msg.voltages[cell] = (battery_status.voltage_v / battery_status.cell_count) * 1000.0f; } else { @@ -851,15 +848,15 @@ class MavlinkStreamHighresIMU : public MavlinkStream mavlink_highres_imu_t msg = {}; msg.time_usec = sensor.timestamp; - msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_x_bias; - msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_y_bias; - msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_z_bias; - msg.xgyro = sensor.gyro_rad[0] - bias.gyro_x_bias; - msg.ygyro = sensor.gyro_rad[1] - bias.gyro_y_bias; - msg.zgyro = sensor.gyro_rad[2] - bias.gyro_z_bias; - msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_x_bias; - msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_y_bias; - msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_z_bias; + msg.xacc = sensor.accelerometer_m_s2[0] - bias.accel_bias[0]; + msg.yacc = sensor.accelerometer_m_s2[1] - bias.accel_bias[1]; + msg.zacc = sensor.accelerometer_m_s2[2] - bias.accel_bias[2]; + msg.xgyro = sensor.gyro_rad[0] - bias.gyro_bias[0]; + msg.ygyro = sensor.gyro_rad[1] - bias.gyro_bias[1]; + msg.zgyro = sensor.gyro_rad[2] - bias.gyro_bias[2]; + msg.xmag = magnetometer.magnetometer_ga[0] - bias.mag_bias[0]; + msg.ymag = magnetometer.magnetometer_ga[1] - bias.mag_bias[1]; + msg.zmag = magnetometer.magnetometer_ga[2] - bias.mag_bias[2]; msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; msg.pressure_alt = air_data.baro_alt_meter; @@ -1186,7 +1183,9 @@ class MavlinkStreamAttitude : public MavlinkStream private: MavlinkOrbSubscription *_att_sub; - uint64_t _att_time; + MavlinkOrbSubscription *_angular_velocity_sub; + uint64_t _att_time{0}; + uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &) = delete; @@ -1196,23 +1195,30 @@ class MavlinkStreamAttitude : public MavlinkStream protected: explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _att_time(0) + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att; + bool updated = false; + + vehicle_attitude_s att{}; + vehicle_angular_velocity_s angular_velocity{}; + updated |= _att_sub->update(&_att_time, &att); + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); - if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_t msg = {}; - matrix::Eulerf euler = matrix::Quatf(att.q); - msg.time_boot_ms = att.timestamp / 1000; + if (updated) { + mavlink_attitude_t msg{}; + + const matrix::Eulerf euler = matrix::Quatf(att.q); + msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; msg.roll = euler.phi(); msg.pitch = euler.theta(); msg.yaw = euler.psi(); - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg); @@ -1259,7 +1265,9 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream private: MavlinkOrbSubscription *_att_sub; - uint64_t _att_time; + MavlinkOrbSubscription *_angular_velocity_sub; + uint64_t _att_time{0}; + uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &) = delete; @@ -1268,24 +1276,29 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _att_time(0) + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att; + bool updated = false; + + vehicle_attitude_s att{}; + vehicle_angular_velocity_s angular_velocity{}; + updated |= _att_sub->update(&_att_time, &att); + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); - if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_quaternion_t msg = {}; + if (updated) { + mavlink_attitude_quaternion_t msg{}; - msg.time_boot_ms = att.timestamp / 1000; + msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; msg.q1 = att.q[0]; msg.q2 = att.q[1]; msg.q3 = att.q[2]; msg.q4 = att.q[3]; - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg); @@ -1296,7 +1309,6 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream } }; - class MavlinkStreamVFRHUD : public MavlinkStream { public: @@ -2439,7 +2451,7 @@ class MavlinkStreamOdometry : public MavlinkStream if (odom_updated) { msg.time_usec = odom.timestamp; - msg.child_frame_id = MAV_FRAME_BODY_NED; + msg.child_frame_id = MAV_FRAME_BODY_FRD; // Current position msg.x = odom.x; @@ -2452,11 +2464,11 @@ class MavlinkStreamOdometry : public MavlinkStream msg.q[2] = odom.q[2]; msg.q[3] = odom.q[3]; - // Local NED to body-NED Dcm matrix - matrix::Dcmf Rlb(matrix::Quatf(odom.q)); + // Body-FRD frame to local NED frame Dcm matrix + matrix::Dcmf R_body_to_local(matrix::Quatf(odom.q)); // Rotate linear and angular velocity from local NED to body-NED frame - matrix::Vector3f linvel_body(Rlb * matrix::Vector3f(odom.vx, odom.vy, odom.vz)); + matrix::Vector3f linvel_body(R_body_to_local.transpose() * matrix::Vector3f(odom.vx, odom.vy, odom.vz)); // Current linear velocity msg.vx = linvel_body(0); @@ -3695,6 +3707,23 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; + switch (traj_wp_avoidance_desired.waypoints[i].type) { + case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; + break; + + case position_setpoint_s::SETPOINT_TYPE_LOITER: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; + break; + + case position_setpoint_s::SETPOINT_TYPE_LAND: + msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + break; + + default: + msg.command[i] = UINT16_MAX; + } + if (traj_wp_avoidance_desired.waypoints[i].point_valid) { number_valid_points++; } @@ -4736,10 +4765,15 @@ class MavlinkStreamGroundTruth : public MavlinkStream } private: + MavlinkOrbSubscription *_angular_velocity_sub; MavlinkOrbSubscription *_att_sub; MavlinkOrbSubscription *_gpos_sub; - uint64_t _att_time; - uint64_t _gpos_time; + MavlinkOrbSubscription *_lpos_sub; + + uint64_t _angular_velocity_time{0}; + uint64_t _att_time{0}; + uint64_t _gpos_time{0}; + uint64_t _lpos_time{0}; /* do not allow top copying this class */ MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &) = delete; @@ -4747,20 +4781,27 @@ class MavlinkStreamGroundTruth : public MavlinkStream protected: explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink), + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity_groundtruth))), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))), _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))), - _att_time(0), - _gpos_time(0) + _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_groundtruth))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att = {}; - vehicle_global_position_s gpos = {}; - bool att_updated = _att_sub->update(&_att_time, &att); - bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos); + bool updated = false; + + vehicle_angular_velocity_s angular_velocity{}; + vehicle_attitude_s att{}; + vehicle_global_position_s gpos{}; + vehicle_local_position_s lpos{}; - if (att_updated || gpos_updated) { + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); + updated |= _att_sub->update(&_att_time, &att); + updated |= _gpos_sub->update(&_gpos_time, &gpos); + updated |= _lpos_sub->update(&_lpos_time, &lpos); + + if (updated) { mavlink_hil_state_quaternion_t msg = {}; // vehicle_attitude -> hil_state_quaternion @@ -4768,23 +4809,23 @@ class MavlinkStreamGroundTruth : public MavlinkStream msg.attitude_quaternion[1] = att.q[1]; msg.attitude_quaternion[2] = att.q[2]; msg.attitude_quaternion[3] = att.q[3]; - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; // vehicle_global_position -> hil_state_quaternion // same units as defined in mavlink/common.xml msg.lat = gpos.lat * 1e7; msg.lon = gpos.lon * 1e7; msg.alt = gpos.alt * 1e3f; - msg.vx = gpos.vel_n * 1e2f; - msg.vy = gpos.vel_e * 1e2f; - msg.vz = gpos.vel_d * 1e2f; + msg.vx = lpos.vx * 1e2f; + msg.vy = lpos.vy * 1e2f; + msg.vz = lpos.vz * 1e2f; msg.ind_airspeed = 0; msg.true_airspeed = 0; - msg.xacc = 0; - msg.yacc = 0; - msg.zacc = 0; + msg.xacc = lpos.ax; + msg.yacc = lpos.ay; + msg.zacc = lpos.az; mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b76d09813738..8e59e6fee1d8 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -76,11 +76,6 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : init_offboard_mission(); } -MavlinkMissionManager::~MavlinkMissionManager() -{ - orb_unadvertise(_offboard_mission_pub); -} - void MavlinkMissionManager::init_offboard_mission() { @@ -185,12 +180,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun _my_dataman_id = _dataman_id; /* mission state saved successfully, publish offboard_mission topic */ - if (_offboard_mission_pub == nullptr) { - _offboard_mission_pub = orb_advertise(ORB_ID(mission), &mission); - - } else { - orb_publish(ORB_ID(mission), _offboard_mission_pub, &mission); - } + _offboard_mission_pub.publish(mission); return PX4_OK; @@ -270,7 +260,6 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } - void MavlinkMissionManager::send_mission_current(uint16_t seq) { diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 2a88d68eea06..fe3f9bfa7e29 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -46,6 +46,7 @@ #pragma once #include +#include #include #include @@ -78,7 +79,7 @@ class MavlinkMissionManager public: explicit MavlinkMissionManager(Mavlink *mavlink); - ~MavlinkMissionManager(); + ~MavlinkMissionManager() = default; /** * Handle sending of messages. Call this regularly at a fixed frequency. @@ -127,7 +128,7 @@ class MavlinkMissionManager uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - orb_advert_t _offboard_mission_pub{nullptr}; + uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index d2ac9d2c5a6e..3173accaa343 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -50,7 +50,7 @@ MavlinkOrbSubscription::is_published() } else if (!published && _subscribe_from_beginning) { // For some topics like vehicle_command_ack, we want to subscribe // from the beginning in order not to miss or delay the first publish respective advertise. - return _sub.forceInit(); + return _sub.subscribe(); } return false; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 1290c9a5f750..d20c84ee58df 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -50,13 +50,6 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : { } -MavlinkParametersManager::~MavlinkParametersManager() -{ - if (_uavcan_parameter_request_pub) { - orb_unadvertise(_uavcan_parameter_request_pub); - } -} - unsigned MavlinkParametersManager::get_size() { @@ -91,12 +84,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) req.node_id = req_list.target_component; req.param_index = 0; - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); } break; @@ -163,12 +151,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) req.int_value = val; } - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); } break; @@ -273,14 +256,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } _rc_param_map.timestamp = hrt_absolute_time(); - - if (_rc_param_map_pub == nullptr) { - _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); - - } else { - orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); - } - + _rc_param_map_pub.publish(_rc_param_map); } break; @@ -383,19 +359,20 @@ bool MavlinkParametersManager::send_uavcan() { /* Send parameter values received from the UAVCAN topic */ - if (_uavcan_parameter_value_sub.updated()) { - uavcan_parameter_value_s value; - _uavcan_parameter_value_sub.update(&value); + uavcan_parameter_value_s value{}; + + if (_uavcan_parameter_value_sub.update(&value)) { // Check if we received a matching parameter, drop it from the list and request the next - if (_uavcan_open_request_list != nullptr - && value.param_index == _uavcan_open_request_list->req.param_index - && value.node_id == _uavcan_open_request_list->req.node_id) { + if ((_uavcan_open_request_list != nullptr) + && (value.param_index == _uavcan_open_request_list->req.param_index) + && (value.node_id == _uavcan_open_request_list->req.node_id)) { + dequeue_uavcan_request(); request_next_uavcan_parameter(); } - mavlink_param_value_t msg; + mavlink_param_value_t msg{}; msg.param_count = value.param_count; msg.param_index = value.param_index; #if defined(__GNUC__) && __GNUC__ >= 8 @@ -424,7 +401,7 @@ MavlinkParametersManager::send_uavcan() } // Re-pack the message with the UAVCAN node ID - mavlink_message_t mavlink_packet; + mavlink_message_t mavlink_packet{}; mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, &msg); _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); @@ -576,12 +553,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter() if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) { uavcan_parameter_request_s req = _uavcan_open_request_list->req; - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + _uavcan_parameter_request_pub.publish(req); _uavcan_waiting_for_request_response = true; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 6c82a42cd838..ea43a14ae31e 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -45,7 +45,8 @@ #include #include "mavlink_bridge_header.h" -#include +#include +#include #include #include #include @@ -59,7 +60,7 @@ class MavlinkParametersManager { public: explicit MavlinkParametersManager(Mavlink *mavlink); - ~MavlinkParametersManager(); + ~MavlinkParametersManager() = default; /** * Handle sending of messages. Call this regularly at a fixed frequency. @@ -125,10 +126,11 @@ class MavlinkParametersManager bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response uint16_t _uavcan_queued_request_items{0}; ///< Number of stored parameter requests currently in the list - orb_advert_t _rc_param_map_pub{nullptr}; + uORB::Publication _rc_param_map_pub{ORB_ID(rc_parameter_map)}; rc_parameter_map_s _rc_param_map{}; - orb_advert_t _uavcan_parameter_request_pub{nullptr}; + uORB::PublicationQueued _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; + uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)}; uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 59023cd659e7..7a8b46823c7a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * 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 @@ -43,12 +43,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -87,31 +81,24 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), _mavlink_ftp(parent), _mavlink_log_handler(parent), - _mavlink_timesync(parent), _mission_manager(parent), - _parameters_manager(parent) + _parameters_manager(parent), + _mavlink_timesync(parent) { - /* Make the attitude quaternion valid */ - _att.q[0] = 1.0f; } void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; + command_ack.timestamp = hrt_absolute_time(); command_ack.command = command; command_ack.result = result; command_ack.target_system = sysid; command_ack.target_component = compid; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); } void @@ -150,6 +137,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_set_position_target_local_ned(msg); break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + handle_message_set_position_target_global_int(msg); + break; + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: handle_message_set_attitude_target(msg); break; @@ -392,7 +383,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -421,7 +413,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) mavlink_command_int_t cmd_mavlink; mavlink_msg_command_int_decode(msg, &cmd_mavlink); - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; vcmd.timestamp = hrt_absolute_time(); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ @@ -515,12 +507,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const } if (!send_ack) { - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vehicle_command); - } + _cmd_pub.publish(vehicle_command); } } @@ -535,9 +522,10 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); - MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); + + vehicle_command_ack_s command_ack{}; - vehicle_command_ack_s command_ack = {}; command_ack.timestamp = hrt_absolute_time(); command_ack.result_param2 = ack.result_param2; command_ack.command = ack.command; @@ -547,13 +535,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) command_ack.target_system = ack.target_system; command_ack.target_component = ack.target_component; - if (_command_ack_pub == nullptr) { - _command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &command_ack); - } + _cmd_ack_pub.publish(command_ack); // TODO: move it to the same place that sent the command if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) { @@ -570,7 +552,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - optical_flow_s f = {}; + optical_flow_s f{}; f.timestamp = hrt_absolute_time(); f.time_since_last_sonar_update = flow.time_delta_distance_us; @@ -596,17 +578,13 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val); rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral); - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } + _flow_pub.publish(f); /* Use distance value for distance sensor topic */ - distance_sensor_s d = {}; - if (flow.distance > 0.0f) { // negative values signal invalid data + + distance_sensor_s d{}; + d.timestamp = f.timestamp; d.min_distance = 0.3f; d.max_distance = 5.0f; @@ -616,13 +594,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.0; - if (_flow_distance_sensor_pub == nullptr) { - _flow_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _flow_distance_sensor_pub, &d); - } + _flow_distance_sensor_pub.publish(d); } } @@ -633,8 +605,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); + optical_flow_s f{}; f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; f.integration_timespan = flow.integration_time_us; @@ -649,16 +620,10 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; - if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &f); - } + _flow_pub.publish(f); /* Use distance value for distance sensor topic */ - struct distance_sensor_s d; - memset(&d, 0, sizeof(d)); + distance_sensor_s d{}; d.timestamp = hrt_absolute_time(); d.min_distance = 0.3f; @@ -669,13 +634,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; d.variance = 0.0; - if (_hil_distance_sensor_pub == nullptr) { - _hil_distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _hil_distance_sensor_pub, &d); - } + _flow_distance_sensor_pub.publish(d); } void @@ -687,7 +646,8 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) union px4_custom_mode custom_mode; custom_mode.data = new_mode.custom_mode; - vehicle_command_s vcmd = {}; + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -703,12 +663,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.confirmation = true; vcmd.from_external = true; - if (_cmd_pub == nullptr) { - _cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } + _cmd_pub.publish(vcmd); } void @@ -718,8 +673,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) mavlink_distance_sensor_t dist_sensor; mavlink_msg_distance_sensor_decode(msg, &dist_sensor); - struct distance_sensor_s d; - memset(&d, 0, sizeof(d)); + distance_sensor_s d{}; d.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ d.min_distance = float(dist_sensor.min_distance) * 1e-2f; /* cm to m */ @@ -731,14 +685,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) d.variance = dist_sensor.covariance * 1e-4f; // cm^2 to m^2 /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum - - if (_distance_sensor_pub == nullptr) { - _distance_sensor_pub = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); - } + _distance_sensor_pub.publish(d); } void @@ -747,7 +694,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mavlink_att_pos_mocap_t mocap; mavlink_msg_att_pos_mocap_decode(msg, &mocap); - struct vehicle_odometry_s mocap_odom = {}; + vehicle_odometry_s mocap_odom{}; mocap_odom.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec); mocap_odom.x = mocap.x; @@ -774,9 +721,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg) mocap_odom.yawspeed = NAN; mocap_odom.velocity_covariance[0] = NAN; - int instance_id = 0; - - orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &mocap_odom, &instance_id, ORB_PRIO_HIGH); + _mocap_odometry_pub.publish(mocap_odom); } void @@ -785,9 +730,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t mavlink_set_position_target_local_ned_t set_position_target_local_ned; mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned); - struct offboard_control_mode_s offboard_control_mode = {}; - - bool values_finite = + const bool values_finite = PX4_ISFINITE(set_position_target_local_ned.x) && PX4_ISFINITE(set_position_target_local_ned.y) && PX4_ISFINITE(set_position_target_local_ned.z) && @@ -806,39 +749,37 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t set_position_target_local_ned.target_component == 0) && values_finite) { + offboard_control_mode_s offboard_control_mode{}; + /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4); offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); - bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); - /* yaw ignore flag mapps to ignore_attitude */ offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800); offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800); + /* yaw ignore flag mapps to ignore_attitude */ + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000); bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000); bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000); bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000); offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode and offboard control loop through is enabled * also publish the setpoint topic which is read by the controller */ if (_mavlink->get_forward_externalsp()) { - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { if (is_force_sp && offboard_control_mode.ignore_position && offboard_control_mode.ignore_velocity) { @@ -846,7 +787,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ - struct position_setpoint_triplet_s pos_sp_triplet = {}; + position_setpoint_triplet_s pos_sp_triplet{}; + pos_sp_triplet.timestamp = hrt_absolute_time(); pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; @@ -872,6 +814,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values */ if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; @@ -883,13 +826,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values */ if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; - pos_sp_triplet.current.velocity_frame = - set_position_target_local_ned.coordinate_frame; + pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame; } else { pos_sp_triplet.current.velocity_valid = false; @@ -906,12 +849,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; - pos_sp_triplet.current.acceleration_is_force = - is_force_sp; + pos_sp_triplet.current.acceleration_is_force = is_force_sp; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -930,6 +873,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (!(offboard_control_mode.ignore_bodyrate_x || offboard_control_mode.ignore_bodyrate_y || offboard_control_mode.ignore_bodyrate_z)) { + pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; @@ -938,20 +882,186 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } //XXX handle global pos setpoints (different MAV frames) + _pos_sp_triplet_pub.publish(pos_sp_triplet); + } + } + } + } +} + +void +MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t *msg) +{ + mavlink_set_position_target_global_int_t set_position_target_global_int; + mavlink_msg_set_position_target_global_int_decode(msg, &set_position_target_global_int); + + const bool values_finite = + PX4_ISFINITE(set_position_target_global_int.alt) && + PX4_ISFINITE(set_position_target_global_int.vx) && + PX4_ISFINITE(set_position_target_global_int.vy) && + PX4_ISFINITE(set_position_target_global_int.vz) && + PX4_ISFINITE(set_position_target_global_int.afx) && + PX4_ISFINITE(set_position_target_global_int.afy) && + PX4_ISFINITE(set_position_target_global_int.afz) && + PX4_ISFINITE(set_position_target_global_int.yaw); + + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == set_position_target_global_int.target_system || + set_position_target_global_int.target_system == 0) && + (mavlink_system.compid == set_position_target_global_int.target_component || + set_position_target_global_int.target_component == 0) && + values_finite) { + + offboard_control_mode_s offboard_control_mode{}; + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + offboard_control_mode.ignore_position = (bool)(set_position_target_global_int.type_mask & + (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_X_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Y_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE)); + offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask & 0x4); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask & + (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VX_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VY_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE)); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask & + (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AX_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AY_IGNORE + | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask & + POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_IGNORE); + offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask & + POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); + offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask & + POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); + offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask & + POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE); + + bool is_force_sp = (bool)(set_position_target_global_int.type_mask & (1 << 9)); + + offboard_control_mode.timestamp = hrt_absolute_time(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); + + if (control_mode.flag_control_offboard_enabled) { + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { + + PX4_WARN("force setpoint not supported"); + + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + position_setpoint_triplet_s pos_sp_triplet{}; + + pos_sp_triplet.timestamp = hrt_absolute_time(); + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + + /* Order of statements matters. Takeoff can override loiter. + * See https://github.com/mavlink/mavlink/pull/670 for a broader conversation. */ + if (set_position_target_global_int.type_mask & 0x3000) { //Loiter setpoint + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + + } else if (set_position_target_global_int.type_mask & 0x1000) { //Takeoff setpoint + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - if (_pos_sp_triplet_pub == nullptr) { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); + } else if (set_position_target_global_int.type_mask & 0x2000) { //Land setpoint + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + + } else if (set_position_target_global_int.type_mask & 0x4000) { //Idle setpoint + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; } else { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; } - } + /* set the local pos values */ + vehicle_local_position_s local_pos{}; + + if (!offboard_control_mode.ignore_position && _vehicle_local_position_sub.copy(&local_pos)) { + if (!globallocalconverter_initialized()) { + globallocalconverter_init(local_pos.ref_lat, local_pos.ref_lon, + local_pos.ref_alt, local_pos.ref_timestamp); + pos_sp_triplet.current.position_valid = false; + + } else { + globallocalconverter_tolocal(set_position_target_global_int.lat_int / 1e7, + set_position_target_global_int.lon_int / 1e7, set_position_target_global_int.alt, + &pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z); + pos_sp_triplet.current.position_valid = true; + } - } + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local velocity values */ + if (!offboard_control_mode.ignore_velocity) { + + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = set_position_target_global_int.vx; + pos_sp_triplet.current.vy = set_position_target_global_int.vy; + pos_sp_triplet.current.vz = set_position_target_global_int.vz; + + pos_sp_triplet.current.velocity_frame = set_position_target_global_int.coordinate_frame; + + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + if (!offboard_control_mode.ignore_alt_hold) { + pos_sp_triplet.current.alt_valid = true; + + } else { + pos_sp_triplet.current.alt_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (!offboard_control_mode.ignore_acceleration_force) { + + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = set_position_target_global_int.afx; + pos_sp_triplet.current.a_y = set_position_target_global_int.afy; + pos_sp_triplet.current.a_z = set_position_target_global_int.afz; + pos_sp_triplet.current.acceleration_is_force = is_force_sp; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw setpoint */ + if (!offboard_control_mode.ignore_attitude) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = set_position_target_global_int.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + /* set the yawrate sp value */ + if (!(offboard_control_mode.ignore_bodyrate_x || + offboard_control_mode.ignore_bodyrate_y || + offboard_control_mode.ignore_bodyrate_z)) { + + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = set_position_target_global_int.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + + _pos_sp_triplet_pub.publish(pos_sp_triplet); + } + } } } } @@ -962,10 +1072,6 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m mavlink_set_actuator_control_target_t set_actuator_control_target; mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target); - struct offboard_control_mode_s offboard_control_mode = {}; - - struct actuator_controls_s actuator_controls = {}; - bool values_finite = PX4_ISFINITE(set_actuator_control_target.controls[0]) && PX4_ISFINITE(set_actuator_control_target.controls[1]) && @@ -981,8 +1087,12 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m (mavlink_system.compid == set_actuator_control_target.target_component || set_actuator_control_target.target_component == 0) && values_finite) { + /* Ignore all setpoints except when controlling the gimbal(group_mlx==2) as we are setting raw actuators here */ bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2); + + offboard_control_mode_s offboard_control_mode{}; + offboard_control_mode.ignore_thrust = ignore_setpoints; offboard_control_mode.ignore_attitude = ignore_setpoints; offboard_control_mode.ignore_bodyrate_x = ignore_setpoints; @@ -994,18 +1104,15 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m offboard_control_mode.timestamp = hrt_absolute_time(); - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); - - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode, publish the actuator controls */ - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { + actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); /* Set duty cycles for the servos in the actuator_controls message */ @@ -1015,43 +1122,19 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m switch (set_actuator_control_target.group_mlx) { case 0: - if (_actuator_controls_pubs[0] == nullptr) { - _actuator_controls_pubs[0] = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_0), _actuator_controls_pubs[0], &actuator_controls); - } - + _actuator_controls_pubs[0].publish(actuator_controls); break; case 1: - if (_actuator_controls_pubs[1] == nullptr) { - _actuator_controls_pubs[1] = orb_advertise(ORB_ID(actuator_controls_1), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_1), _actuator_controls_pubs[1], &actuator_controls); - } - + _actuator_controls_pubs[1].publish(actuator_controls); break; case 2: - if (_actuator_controls_pubs[2] == nullptr) { - _actuator_controls_pubs[2] = orb_advertise(ORB_ID(actuator_controls_2), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_pubs[2], &actuator_controls); - } - + _actuator_controls_pubs[2].publish(actuator_controls); break; case 3: - if (_actuator_controls_pubs[3] == nullptr) { - _actuator_controls_pubs[3] = orb_advertise(ORB_ID(actuator_controls_3), &actuator_controls); - - } else { - orb_publish(ORB_ID(actuator_controls_3), _actuator_controls_pubs[3], &actuator_controls); - } - + _actuator_controls_pubs[3].publish(actuator_controls); break; default: @@ -1083,7 +1166,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) mavlink_vision_position_estimate_t ev; mavlink_msg_vision_position_estimate_decode(msg, &ev); - struct vehicle_odometry_s visual_odom = {}; + vehicle_odometry_s visual_odom{}; visual_odom.timestamp = _mavlink_timesync.sync_stamp(ev.usec); visual_odom.x = ev.x; @@ -1114,8 +1197,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) visual_odom.yawspeed = NAN; visual_odom.velocity_covariance[0] = NAN; - int instance_id = 0; - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odom, &instance_id, ORB_PRIO_HIGH); + _visual_odometry_pub.publish(visual_odom); } void @@ -1128,15 +1210,16 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) odometry.timestamp = _mavlink_timesync.sync_stamp(odom.time_usec); - /* The position is in the local NED frame */ + /* The position is in a local FRD frame */ odometry.x = odom.x; odometry.y = odom.y; odometry.z = odom.z; - /* The quaternion of the ODOMETRY msg represents a rotation from NED - * earth/local frame to XYZ body frame */ - const matrix::Quatf q(odom.q); - q.copyTo(odometry.q); + /* The quaternion of the ODOMETRY msg represents a rotation from body frame to + * a local frame*/ + matrix::Quatf q_body_to_local(odom.q); + q_body_to_local.normalize(); + q_body_to_local.copyTo(odometry.q); // TODO: // - add a MAV_FRAME_*_OTHER to the Mavlink MAV_FRAME enum IOT define @@ -1159,37 +1242,20 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) odometry.pose_covariance[i] = odom.pose_covariance[i]; } - if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { /* WRT to estimated vehicle body-fixed frame */ - /* get quaternion from the msg quaternion itself and build DCM matrix from it */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I(); - - /* the linear velocities needs to be transformed to the local NED frame */ - matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); - odometry.vx = linvel_local(0); - odometry.vy = linvel_local(1); - odometry.vz = linvel_local(2); - odometry.rollspeed = odom.rollspeed; - odometry.pitchspeed = odom.pitchspeed; - odometry.yawspeed = odom.yawspeed; - - //TODO: Apply rotation matrix to transform from body-fixed NED to earth-fixed NED frame - for (size_t i = 0; i < VEL_URT_SIZE; i++) { - odometry.velocity_covariance[i] = odom.velocity_covariance[i]; - } - - } else if (odom.child_frame_id == MAV_FRAME_BODY_FLU) { /* WRT to estimated vehicle body-fixed frame */ - /* get quaternion from the msg quaternion itself and build DCM matrix from it */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(odometry.q)).I(); - - /* the position needs to be transformed to the local NED frame */ - matrix::Vector3f pos(Rbl * matrix::Vector3(odom.x, -odom.y, -odom.z)); - odometry.x = pos(0); - odometry.y = pos(1); - odometry.z = pos(2); + /* + * PX4 expects the body's linear velocity in the local frame, + * the linear velocity is rotated from the odom child_frame to the + * local NED frame. The angular velocity needs to be expressed in the + * body (fcu_frd) frame. + */ + if (odom.child_frame_id == MAV_FRAME_BODY_FRD) { + /* Linear velocity has to be rotated to the local NED frame + * Angular velocities are already in the right body frame */ + const matrix::Dcmf R_body_to_local = matrix::Dcmf(q_body_to_local); /* the linear velocities needs to be transformed to the local NED frame */ - matrix::Vector3f linvel_local(Rbl * matrix::Vector3(odom.vx, -odom.vy, -odom.vz)); + matrix::Vector3 linvel_local(R_body_to_local * matrix::Vector3(odom.vx, odom.vy, odom.vz)); odometry.vx = linvel_local(0); odometry.vy = linvel_local(1); odometry.vz = linvel_local(2); @@ -1203,65 +1269,15 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) odometry.velocity_covariance[i] = odom.velocity_covariance[i]; } - } else if (odom.child_frame_id == MAV_FRAME_BODY_NED) { /* WRT to vehicle body-NED frame */ - if (_vehicle_attitude_sub.copy(&_att)) { - - /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ - const matrix::Dcmf Rbl = matrix::Dcmf(matrix::Quatf(_att.q)).I(); - - /* the linear velocities needs to be transformed to the local NED frame */ - matrix::Vector3 linvel_local(Rbl * matrix::Vector3(odom.vx, odom.vy, odom.vz)); - odometry.vx = linvel_local(0); - odometry.vy = linvel_local(1); - odometry.vz = linvel_local(2); - - odometry.rollspeed = odom.rollspeed; - odometry.pitchspeed = odom.pitchspeed; - odometry.yawspeed = odom.yawspeed; - - //TODO: Apply rotation matrix to transform from body-fixed to earth-fixed NED frame - for (size_t i = 0; i < VEL_URT_SIZE; i++) { - odometry.velocity_covariance[i] = odom.velocity_covariance[i]; - } - - } - - } else if (odom.child_frame_id == MAV_FRAME_VISION_NED || /* WRT to vehicle local NED frame */ - odom.child_frame_id == MAV_FRAME_MOCAP_NED) { - - if (_vehicle_attitude_sub.copy(&_att)) { - - /* get quaternion from vehicle_attitude quaternion and build DCM matrix from it */ - matrix::Dcmf Rlb = matrix::Quatf(_att.q); - - odometry.vx = odom.vx; - odometry.vy = odom.vy; - odometry.vz = odom.vz; - - /* the angular rates need to be transformed to the body frame */ - matrix::Vector3 angvel_local(Rlb * matrix::Vector3(odom.rollspeed, odom.pitchspeed, odom.yawspeed)); - odometry.rollspeed = angvel_local(0); - odometry.pitchspeed = angvel_local(1); - odometry.yawspeed = angvel_local(2); - - //TODO: Apply rotation matrix to transform from earth-fixed to body-fixed NED frame - for (size_t i = 0; i < VEL_URT_SIZE; i++) { - odometry.velocity_covariance[i] = odom.velocity_covariance[i]; - } - - } - } else { PX4_ERR("Body frame %u not supported. Unable to publish velocity", odom.child_frame_id); } - int instance_id = 0; - if (odom.frame_id == MAV_FRAME_VISION_NED) { - orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + _visual_odometry_pub.publish(odometry); } else if (odom.frame_id == MAV_FRAME_MOCAP_NED) { - orb_publish_auto(ORB_ID(vehicle_mocap_odometry), &_mocap_odometry_pub, &odometry, &instance_id, ORB_PRIO_HIGH); + _mocap_odometry_pub.publish(odometry); } else { PX4_ERR("Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id); @@ -1291,8 +1307,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) set_attitude_target.target_component == 0) && values_finite) { + offboard_control_mode_s offboard_control_mode{}; + /* set correct ignore flags for thrust field: copy from mavlink message */ - _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust @@ -1314,46 +1332,40 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y || ignore_bodyrate_msg_z) && - ignore_attitude_msg && !_offboard_control_mode.ignore_thrust) { + ignore_attitude_msg && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - _offboard_control_mode.ignore_bodyrate_x = - ignore_bodyrate_msg_x && _offboard_control_mode.ignore_bodyrate_x; - _offboard_control_mode.ignore_bodyrate_y = - ignore_bodyrate_msg_y && _offboard_control_mode.ignore_bodyrate_y; - _offboard_control_mode.ignore_bodyrate_z = - ignore_bodyrate_msg_z && _offboard_control_mode.ignore_bodyrate_z; - _offboard_control_mode.ignore_attitude = ignore_attitude_msg && _offboard_control_mode.ignore_attitude; + offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x; + offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y; + offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z; + offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude; } else { - _offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; - _offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; - _offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; - _offboard_control_mode.ignore_attitude = ignore_attitude_msg; + offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x; + offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y; + offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z; + offboard_control_mode.ignore_attitude = ignore_attitude_msg; } - _offboard_control_mode.ignore_position = true; - _offboard_control_mode.ignore_velocity = true; - _offboard_control_mode.ignore_acceleration_force = true; + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; - _offboard_control_mode.timestamp = hrt_absolute_time(); - - if (_offboard_control_mode_pub == nullptr) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); + offboard_control_mode.timestamp = hrt_absolute_time(); - } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); - } + _offboard_control_mode_pub.publish(offboard_control_mode); /* If we are in offboard control mode and offboard control loop through is enabled * also publish the setpoint topic which is read by the controller */ if (_mavlink->get_forward_externalsp()) { - _control_mode_sub.update(&_control_mode); + vehicle_control_mode_s control_mode{}; + _control_mode_sub.copy(&control_mode); - if (_control_mode.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(_offboard_control_mode.ignore_attitude)) { + if (!(offboard_control_mode.ignore_attitude)) { vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); @@ -1369,7 +1381,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.yaw_sp_move_rate = 0.0f; } - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid // Fill correct field by checking frametype // TODO: add as needed switch (_mavlink->get_system_type()) { @@ -1394,19 +1406,16 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - if (_att_sp_pub == nullptr) { - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - } else { - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - } + _att_sp_pub.publish(att_sp); } /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ - if (!_offboard_control_mode.ignore_bodyrate_x || - !_offboard_control_mode.ignore_bodyrate_y || - !_offboard_control_mode.ignore_bodyrate_z) { - vehicle_rates_setpoint_s rates_sp = {}; + if (!offboard_control_mode.ignore_bodyrate_x || + !offboard_control_mode.ignore_bodyrate_y || + !offboard_control_mode.ignore_bodyrate_z) { + + vehicle_rates_setpoint_s rates_sp{}; + rates_sp.timestamp = hrt_absolute_time(); // only copy att rates sp if message contained new data @@ -1422,7 +1431,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) rates_sp.yaw = set_attitude_target.body_yaw_rate; } - if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid switch (_mavlink->get_system_type()) { case MAV_TYPE_GENERIC: break; @@ -1446,15 +1455,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } - if (_rates_sp_pub == nullptr) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); - } + _rates_sp_pub.publish(rates_sp); } } - } } } @@ -1467,7 +1470,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - radio_status_s status = {}; + radio_status_s status{}; + status.timestamp = hrt_absolute_time(); status.rssi = rstatus.rssi; status.remote_rssi = rstatus.remrssi; @@ -1479,8 +1483,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) _mavlink->update_radio_status(status); - int multi_instance; - orb_publish_auto(ORB_ID(radio_status), &_radio_status_pub, &status, &multi_instance, ORB_PRIO_HIGH); + _radio_status_pub.publish(status); } } @@ -1499,7 +1502,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) } else if ((ping.target_system == mavlink_system.sysid) && (ping.target_component == - mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it. + mavlink_system.compid)) { // This is a returned ping message from this system. Calculate latency from it. const hrt_abstime now = hrt_absolute_time(); @@ -1534,7 +1537,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) /* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { - struct ping_s uorb_ping_msg = {}; + ping_s uorb_ping_msg{}; uorb_ping_msg.timestamp = now; uorb_ping_msg.ping_time = ping.time_usec; @@ -1544,13 +1547,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) uorb_ping_msg.system_id = msg->sysid; uorb_ping_msg.component_id = msg->compid; - if (_ping_pub == nullptr) { - int multi_instance; - _ping_pub = orb_advertise_multi(ORB_ID(ping), &uorb_ping_msg, &multi_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(ping), _ping_pub, &uorb_ping_msg); - } + _ping_pub.publish(uorb_ping_msg); } } } @@ -1567,7 +1564,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) mavlink_battery_status_t battery_mavlink; mavlink_msg_battery_status_decode(msg, &battery_mavlink); - battery_status_s battery_status = {}; + battery_status_s battery_status{}; battery_status.timestamp = hrt_absolute_time(); float voltage_sum = 0.0f; @@ -1599,12 +1596,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; } - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &battery_status); - } + _battery_pub.publish(battery_status); } void @@ -1677,7 +1669,8 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) mavlink_obstacle_distance_t mavlink_obstacle_distance; mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance); - obstacle_distance_s obstacle_distance = {}; + obstacle_distance_s obstacle_distance{}; + obstacle_distance.timestamp = hrt_absolute_time(); obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type; memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances)); @@ -1692,13 +1685,9 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance; obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance; obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset; + obstacle_distance.frame = mavlink_obstacle_distance.frame; - if (_obstacle_distance_pub == nullptr) { - _obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &obstacle_distance); - - } else { - orb_publish(ORB_ID(obstacle_distance), _obstacle_distance_pub, &obstacle_distance); - } + _obstacle_distance_pub.publish(obstacle_distance); } void @@ -1707,8 +1696,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess mavlink_trajectory_representation_waypoints_t trajectory; mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - struct vehicle_trajectory_waypoint_s trajectory_waypoint = {}; + vehicle_trajectory_waypoint_s trajectory_waypoint{}; trajectory_waypoint.timestamp = hrt_absolute_time(); const int number_valid_points = trajectory.valid_points; @@ -1729,6 +1717,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; + trajectory_waypoint.waypoints[i].type = UINT8_MAX; } for (int i = 0; i < number_valid_points; ++i) { @@ -1739,13 +1728,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess trajectory_waypoint.waypoints[i].point_valid = false; } - if (_trajectory_waypoint_pub == nullptr) { - _trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &trajectory_waypoint); - - } else { - orb_publish(ORB_ID(vehicle_trajectory_waypoint), _trajectory_waypoint_pub, &trajectory_waypoint); - } - + _trajectory_waypoint_pub.publish(trajectory_waypoint); } switch_pos_t @@ -1758,7 +1741,6 @@ MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw) int MavlinkReceiver::decode_switch_pos_n(uint16_t buttons, unsigned sw) { - bool on = (buttons & (1 << sw)); if (sw < MOM_SWITCH_COUNT) { @@ -1803,7 +1785,8 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } // fill uORB message - struct input_rc_s rc = {}; + input_rc_s rc{}; + // metadata rc.timestamp = hrt_absolute_time(); rc.timestamp_last_signal = rc.timestamp; @@ -1814,6 +1797,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.rc_total_frame_count = 1; rc.rc_ppm_frame_length = 0; rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; + // channels rc.values[0] = man.chan1_raw; rc.values[1] = man.chan2_raw; @@ -1851,9 +1835,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } // publish uORB message - int instance; // provides the instance ID or the publication - ORB_PRIO priority = ORB_PRIO_HIGH; // since it is an override, set priority high - orb_publish_auto(ORB_ID(input_rc), &_rc_pub, &rc, &instance, priority); + _rc_pub.publish(rc); } void @@ -1869,7 +1851,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) if (_mavlink->get_manual_input_mode_generation()) { - struct input_rc_s rc = {}; + input_rc_s rc{}; rc.timestamp = hrt_absolute_time(); rc.timestamp_last_signal = rc.timestamp; @@ -1882,14 +1864,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK; rc.rssi = RC_INPUT_RSSI_MAX; - /* roll */ - rc.values[0] = man.x / 2 + 1500; - /* pitch */ - rc.values[1] = man.y / 2 + 1500; - /* yaw */ - rc.values[2] = man.r / 2 + 1500; - /* throttle */ - rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); + rc.values[0] = man.x / 2 + 1500; // roll + rc.values[1] = man.y / 2 + 1500; // pitch + rc.values[2] = man.r / 2 + 1500; // yaw + rc.values[3] = math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f); // throttle /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1906,15 +1884,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) _mom_switch_state = man.buttons; - if (_rc_pub == nullptr) { - _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); - - } else { - orb_publish(ORB_ID(input_rc), _rc_pub, &rc); - } + _rc_pub.publish(rc); } else { - struct manual_control_setpoint_s manual = {}; + manual_control_setpoint_s manual{}; manual.timestamp = hrt_absolute_time(); manual.x = man.x / 1000.0f; @@ -1923,8 +1896,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.z = man.z / 1000.0f; manual.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); - int m_inst; - orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_pub, &manual, &m_inst, ORB_PRIO_LOW); + _manual_pub.publish(manual); } } @@ -2018,30 +1990,27 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); /* airspeed */ { - struct airspeed_s airspeed = {}; + airspeed_s airspeed{}; - float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature); + float ias = calc_IAS(imu.diff_pressure * 1e2f); + float scale = 1.0f; //assume no instrument or pitot placement errors + float eas = calc_EAS_from_IAS(ias, scale); + float tas = calc_TAS_from_EAS(eas, imu.abs_pressure * 100, imu.temperature); airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; - if (_airspeed_pub == nullptr) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } + _airspeed_pub.publish(airspeed); } /* gyro */ { - sensor_gyro_s gyro = {}; + sensor_gyro_s gyro{}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -2052,17 +2021,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.z = imu.zgyro; gyro.temperature = imu.temperature; - if (_gyro_pub == nullptr) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } + _gyro_pub.publish(gyro); } /* accelerometer */ { - sensor_accel_s accel = {}; + sensor_accel_s accel{}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f); @@ -2073,17 +2037,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.z = imu.zacc; accel.temperature = imu.temperature; - if (_accel_pub == nullptr) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } + _accel_pub.publish(accel); } /* magnetometer */ { - struct mag_report mag = {}; + sensor_mag_s mag{}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -2093,18 +2052,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.y = imu.ymag; mag.z = imu.zmag; - if (_mag_pub == nullptr) { - /* publish to the first mag topic */ - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } + _mag_pub.publish(mag); } /* baro */ { - sensor_baro_s baro = {}; + sensor_baro_s baro{}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -2113,17 +2066,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* fake device ID */ baro.device_id = 1234567; - if (_baro_pub == nullptr) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } + _baro_pub.publish(baro); } /* battery status */ { - struct battery_status_s hil_battery_status = {}; + battery_status_s hil_battery_status{}; hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.5f; @@ -2131,12 +2079,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } + _battery_pub.publish(hil_battery_status); } } @@ -2146,7 +2089,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) mavlink_hil_gps_t gps; mavlink_msg_hil_gps_decode(msg, &gps); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); struct vehicle_gps_position_s hil_gps = {}; @@ -2175,57 +2118,41 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.heading = NAN; hil_gps.heading_offset = NAN; - if (_gps_pub == nullptr) { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); - - } else { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); - } + _gps_pub.publish(hil_gps); } void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg) { mavlink_follow_target_t follow_target_msg; - follow_target_s follow_target_topic = {}; - mavlink_msg_follow_target_decode(msg, &follow_target_msg); - follow_target_topic.timestamp = hrt_absolute_time(); + follow_target_s follow_target_topic{}; + follow_target_topic.timestamp = hrt_absolute_time(); follow_target_topic.lat = follow_target_msg.lat * 1e-7; follow_target_topic.lon = follow_target_msg.lon * 1e-7; follow_target_topic.alt = follow_target_msg.alt; - if (_follow_target_pub == nullptr) { - _follow_target_pub = orb_advertise(ORB_ID(follow_target), &follow_target_topic); - - } else { - orb_publish(ORB_ID(follow_target), _follow_target_pub, &follow_target_topic); - } + _follow_target_pub.publish(follow_target_topic); } void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg) { mavlink_landing_target_t landing_target; - mavlink_msg_landing_target_decode(msg, &landing_target); if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) { - landing_target_pose_s landing_target_pose = {}; + landing_target_pose_s landing_target_pose{}; + landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec); landing_target_pose.abs_pos_valid = true; landing_target_pose.x_abs = landing_target.x; landing_target_pose.y_abs = landing_target.y; landing_target_pose.z_abs = landing_target.z; - if (_landing_target_pose_pub == nullptr) { - _landing_target_pose_pub = orb_advertise(ORB_ID(landing_target_pose), &landing_target_pose); - - } else { - orb_publish(ORB_ID(landing_target_pose), _landing_target_pose_pub, &landing_target_pose); - } + _landing_target_pose_pub.publish(landing_target_pose); } } @@ -2233,10 +2160,10 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) { mavlink_adsb_vehicle_t adsb; - transponder_report_s t = {}; - mavlink_msg_adsb_vehicle_decode(msg, &adsb); + transponder_report_s t{}; + t.timestamp = hrt_absolute_time(); t.icao_address = adsb.ICAO_address; @@ -2268,20 +2195,13 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) //PX4_INFO("code: %d callsign: %s, vel: %8.4f, tslc: %d", (int)t.ICAO_address, t.callsign, (double)t.hor_velocity, (int)t.tslc); - if (_transponder_report_pub == nullptr) { - _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); - } + _transponder_report_pub.publish(t); } void MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) { - mavlink_utm_global_position_t utm_pos{}; - transponder_report_s t{}; - + mavlink_utm_global_position_t utm_pos; mavlink_msg_utm_global_position_decode(msg, &utm_pos); // Convert cm/s to m/s @@ -2289,6 +2209,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) float vy = utm_pos.vy / 100.0f; float vz = utm_pos.vz / 100.0f; + transponder_report_s t{}; t.timestamp = hrt_absolute_time(); // TODO: ID t.lat = utm_pos.lat * 1e-7; @@ -2337,13 +2258,7 @@ MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not // provide these. - - if (_transponder_report_pub == nullptr) { - _transponder_report_pub = orb_advertise_queue(ORB_ID(transponder_report), &t, transponder_report_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(transponder_report), _transponder_report_pub, &t); - } + _transponder_report_pub.publish(t); _last_utm_global_pos_com = t.timestamp; } @@ -2352,50 +2267,37 @@ void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) { mavlink_collision_t collision; - collision_report_s t = {}; - mavlink_msg_collision_decode(msg, &collision); - t.timestamp = hrt_absolute_time(); - t.src = collision.src; - t.id = collision.id; - t.action = collision.action; - t.threat_level = collision.threat_level; - t.time_to_minimum_delta = collision.time_to_minimum_delta; - t.altitude_minimum_delta = collision.altitude_minimum_delta; - t.horizontal_minimum_delta = collision.horizontal_minimum_delta; + collision_report_s collision_report{}; - if (_collision_report_pub == nullptr) { - _collision_report_pub = orb_advertise(ORB_ID(collision_report), &t); + collision_report.timestamp = hrt_absolute_time(); + collision_report.src = collision.src; + collision_report.id = collision.id; + collision_report.action = collision.action; + collision_report.threat_level = collision.threat_level; + collision_report.time_to_minimum_delta = collision.time_to_minimum_delta; + collision_report.altitude_minimum_delta = collision.altitude_minimum_delta; + collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta; - } else { - orb_publish(ORB_ID(collision_report), _collision_report_pub, &t); - } + _collision_report_pub.publish(collision_report); } void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) { - mavlink_gps_rtcm_data_t gps_rtcm_data_msg = {}; + mavlink_gps_rtcm_data_t gps_rtcm_data_msg; mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg); - gps_inject_data_s gps_inject_data_topic = {}; + gps_inject_data_s gps_inject_data_topic{}; + gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data), (int)sizeof(uint8_t) * gps_rtcm_data_msg.len); gps_inject_data_topic.flags = gps_rtcm_data_msg.flags; memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data, math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len)); - orb_advert_t &pub = _gps_inject_data_pub; - - if (pub == nullptr) { - pub = orb_advertise_queue(ORB_ID(gps_inject_data), &gps_inject_data_topic, - _gps_inject_data_queue_size); - - } else { - orb_publish(ORB_ID(gps_inject_data), pub, &gps_inject_data_topic); - } - + _gps_inject_data_pub.publish(gps_inject_data_topic); } void @@ -2404,48 +2306,34 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) mavlink_hil_state_quaternion_t hil_state; mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - uint64_t timestamp = hrt_absolute_time(); + const uint64_t timestamp = hrt_absolute_time(); /* airspeed */ { - struct airspeed_s airspeed = {}; + airspeed_s airspeed{}; airspeed.timestamp = timestamp; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - if (_airspeed_pub == nullptr) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } + _airspeed_pub.publish(airspeed); } /* attitude */ - struct vehicle_attitude_s hil_attitude; { - hil_attitude = {}; + vehicle_attitude_s hil_attitude{}; + hil_attitude.timestamp = timestamp; matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - - if (_attitude_pub == nullptr) { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); - } + _attitude_pub.publish(hil_attitude); } /* global position */ { - struct vehicle_global_position_s hil_global_pos = {}; + vehicle_global_position_s hil_global_pos{}; hil_global_pos.timestamp = timestamp; hil_global_pos.lat = hil_state.lat / ((double)1e7); @@ -2457,12 +2345,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; - if (_global_pos_pub == nullptr) { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - - } else { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); - } + _global_pos_pub.publish(hil_global_pos); } /* local position */ @@ -2473,64 +2356,47 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(&_hil_local_proj_ref, lat, lon); - _hil_local_pos.ref_timestamp = timestamp; - _hil_local_pos.ref_lat = lat; - _hil_local_pos.ref_lon = lon; - _hil_local_pos.ref_alt = _hil_local_alt0; } float x = 0.0f; float y = 0.0f; map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); - _hil_local_pos.timestamp = timestamp; - _hil_local_pos.xy_valid = true; - _hil_local_pos.z_valid = true; - _hil_local_pos.v_xy_valid = true; - _hil_local_pos.v_z_valid = true; - _hil_local_pos.x = x; - _hil_local_pos.y = y; - _hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; - _hil_local_pos.vx = hil_state.vx / 100.0f; - _hil_local_pos.vy = hil_state.vy / 100.0f; - _hil_local_pos.vz = hil_state.vz / 100.0f; - matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); - _hil_local_pos.yaw = euler.psi(); - _hil_local_pos.xy_global = true; - _hil_local_pos.z_global = true; - _hil_local_pos.vxy_max = INFINITY; - _hil_local_pos.vz_max = INFINITY; - _hil_local_pos.hagl_min = INFINITY; - _hil_local_pos.hagl_max = INFINITY; - - if (_local_pos_pub == nullptr) { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_hil_local_pos); - } else { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_hil_local_pos); - } - } - - /* land detector */ - { - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - - if (_hil_land_detector.landed != landed) { - _hil_land_detector.landed = landed; - _hil_land_detector.timestamp = hrt_absolute_time(); - - if (_land_detector_pub == nullptr) { - _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &_hil_land_detector); - - } else { - orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &_hil_land_detector); - } - } + vehicle_local_position_s hil_local_pos{}; + hil_local_pos.timestamp = timestamp; + + hil_local_pos.ref_timestamp = _hil_local_proj_ref.timestamp; + hil_local_pos.ref_lat = math::radians(_hil_local_proj_ref.lat_rad); + hil_local_pos.ref_lon = math::radians(_hil_local_proj_ref.lon_rad); + hil_local_pos.ref_alt = _hil_local_alt0; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; + + matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; + hil_local_pos.yaw = euler.psi(); + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.vxy_max = INFINITY; + hil_local_pos.vz_max = INFINITY; + hil_local_pos.hagl_min = INFINITY; + hil_local_pos.hagl_max = INFINITY; + + _local_pos_pub.publish(hil_local_pos); } /* accelerometer */ { - sensor_accel_s accel = {}; + sensor_accel_s accel{}; accel.timestamp = timestamp; accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; @@ -2541,17 +2407,28 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.z = hil_state.zacc; accel.temperature = 25.0f; - if (_accel_pub == nullptr) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + _accel_pub.publish(accel); + } - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } + /* gyroscope */ + { + sensor_gyro_s gyro{}; + + gyro.timestamp = timestamp; + gyro.x_raw = hil_state.rollspeed * 1e3f; + gyro.y_raw = hil_state.pitchspeed * 1e3f; + gyro.z_raw = hil_state.yawspeed * 1e3f; + gyro.x = hil_state.rollspeed; + gyro.y = hil_state.pitchspeed; + gyro.z = hil_state.yawspeed; + gyro.temperature = 25.0f; + + _gyro_pub.publish(gyro); } /* battery status */ { - struct battery_status_s hil_battery_status = {}; + battery_status_s hil_battery_status{}; hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; @@ -2559,12 +2436,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub == nullptr) { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - - } else { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - } + _battery_pub.publish(hil_battery_status); } } @@ -2572,51 +2444,41 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg) { mavlink_named_value_float_t debug_msg; - debug_key_value_s debug_topic; - mavlink_msg_named_value_float_decode(msg, &debug_msg); + debug_key_value_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); memcpy(debug_topic.key, debug_msg.name, sizeof(debug_topic.key)); debug_topic.key[sizeof(debug_topic.key) - 1] = '\0'; // enforce null termination debug_topic.value = debug_msg.value; - if (_debug_key_value_pub == nullptr) { - _debug_key_value_pub = orb_advertise(ORB_ID(debug_key_value), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_key_value), _debug_key_value_pub, &debug_topic); - } + _debug_key_value_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg) { mavlink_debug_t debug_msg; - debug_value_s debug_topic; - mavlink_msg_debug_decode(msg, &debug_msg); + debug_value_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); debug_topic.ind = debug_msg.ind; debug_topic.value = debug_msg.value; - if (_debug_value_pub == nullptr) { - _debug_value_pub = orb_advertise(ORB_ID(debug_value), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_value), _debug_value_pub, &debug_topic); - } + _debug_value_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) { mavlink_debug_vect_t debug_msg; - debug_vect_s debug_topic; - mavlink_msg_debug_vect_decode(msg, &debug_msg); + debug_vect_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination @@ -2624,22 +2486,17 @@ MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg) debug_topic.y = debug_msg.y; debug_topic.z = debug_msg.z; - if (_debug_vect_pub == nullptr) { - _debug_vect_pub = orb_advertise(ORB_ID(debug_vect), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_vect), _debug_vect_pub, &debug_topic); - } + _debug_vect_pub.publish(debug_topic); } void MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) { mavlink_debug_float_array_t debug_msg; - debug_array_s debug_topic = {}; - mavlink_msg_debug_float_array_decode(msg, &debug_msg); + debug_array_s debug_topic{}; + debug_topic.timestamp = hrt_absolute_time(); debug_topic.id = debug_msg.array_id; memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name)); @@ -2649,21 +2506,15 @@ MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg) debug_topic.data[i] = debug_msg.data[i]; } - if (_debug_array_pub == nullptr) { - _debug_array_pub = orb_advertise(ORB_ID(debug_array), &debug_topic); - - } else { - orb_publish(ORB_ID(debug_array), _debug_array_pub, &debug_topic); - } + _debug_array_pub.publish(debug_topic); } /** * Receive data from UART/UDP */ -void * -MavlinkReceiver::receive_thread(void *arg) +void +MavlinkReceiver::Run() { - /* set thread name */ { char thread_name[24]; @@ -2755,18 +2606,21 @@ MavlinkReceiver::receive_thread(void *arg) // the system within the first N seconds hrt_abstime stime = _mavlink->get_start_time(); - if ((stime != 0 && (hrt_elapsed_time(&stime) > 3 * 1000 * 1000)) + if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { + srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last.sin_port = srcaddr.sin_port; + _mavlink->set_client_source_initialized(); + PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); } } #endif - // only start accepting messages once we're sure who we talk to + // only start accepting messages once we're sure who we talk to if (_mavlink->get_client_source_initialized()) { /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -2828,26 +2682,15 @@ MavlinkReceiver::receive_thread(void *arg) } } - - return nullptr; } void * MavlinkReceiver::start_helper(void *context) { + MavlinkReceiver rcv{(Mavlink *)context}; + rcv.Run(); - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - - if (!rcv) { - PX4_ERR("alloc failed"); - return nullptr; - } - - void *ret = rcv->receive_thread(nullptr); - - delete rcv; - - return ret; + return nullptr; } void @@ -2861,7 +2704,9 @@ MavlinkReceiver::receive_start(pthread_t *thread, Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, PX4_STACK_ADJUSTED(2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); + pthread_attr_setstacksize(&receiveloop_attr, + PX4_STACK_ADJUSTED(sizeof(MavlinkReceiver) + 2840 + MAVLINK_RECEIVER_NET_ADDED_STACK)); + pthread_create(thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 23982cd5904b..ddfdda261d78 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * 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 @@ -41,12 +41,21 @@ #pragma once -#include +#include "mavlink_ftp.h" +#include "mavlink_log_handler.h" +#include "mavlink_mission.h" +#include "mavlink_parameters.h" +#include "mavlink_timesync.h" -#include +#include +#include +#include +#include #include #include #include +#include +#include #include #include #include @@ -57,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -64,16 +74,22 @@ #include #include #include +#include #include +#include +#include #include +#include +#include #include #include #include #include #include +#include #include -#include #include +#include #include #include #include @@ -82,20 +98,11 @@ #include #include -#include "mavlink_ftp.h" -#include "mavlink_log_handler.h" -#include "mavlink_mission.h" -#include "mavlink_parameters.h" -#include "mavlink_timesync.h" - class Mavlink; class MavlinkReceiver : public ModuleParams { public: - /** - * Constructor - */ MavlinkReceiver(Mavlink *parent); ~MavlinkReceiver() = default; @@ -118,8 +125,8 @@ class MavlinkReceiver : public ModuleParams const vehicle_command_s &vehicle_command); void handle_message(mavlink_message_t *msg); + void handle_message_adsb_vehicle(mavlink_message_t *msg); - void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); void handle_message_battery_status(mavlink_message_t *msg); void handle_message_collision(mavlink_message_t *msg); @@ -154,10 +161,13 @@ class MavlinkReceiver : public ModuleParams void handle_message_set_attitude_target(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_set_position_target_local_ned(mavlink_message_t *msg); + void handle_message_set_position_target_global_int(mavlink_message_t *msg); void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); + void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); - void *receive_thread(void *arg); + + void Run(); /** * Set the interval at which the given message stream is published. @@ -185,7 +195,6 @@ class MavlinkReceiver : public ModuleParams bool evaluate_target_ok(int command, int target_system, int target_component); void send_flight_information(); - void send_storage_information(int storage_id); /** @@ -193,97 +202,87 @@ class MavlinkReceiver : public ModuleParams */ void update_params(); - Mavlink *_mavlink; + Mavlink *_mavlink; MavlinkFTP _mavlink_ftp; MavlinkLogHandler _mavlink_log_handler; - MavlinkTimesync _mavlink_timesync; - MavlinkMissionManager _mission_manager; MavlinkParametersManager _parameters_manager; + MavlinkTimesync _mavlink_timesync; - mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() - - map_projection_reference_s _hil_local_proj_ref {}; - offboard_control_mode_s _offboard_control_mode{}; - - vehicle_attitude_s _att {}; - vehicle_local_position_s _hil_local_pos {}; - vehicle_land_detected_s _hil_land_detector {}; - vehicle_control_mode_s _control_mode {}; - - orb_advert_t _accel_pub{nullptr}; - orb_advert_t _actuator_controls_pubs[4] {nullptr, nullptr, nullptr, nullptr}; - orb_advert_t _airspeed_pub{nullptr}; - orb_advert_t _att_sp_pub{nullptr}; - orb_advert_t _attitude_pub{nullptr}; - orb_advert_t _baro_pub{nullptr}; - orb_advert_t _battery_pub{nullptr}; - orb_advert_t _cmd_pub{nullptr}; - orb_advert_t _collision_report_pub{nullptr}; - orb_advert_t _command_ack_pub{nullptr}; - orb_advert_t _debug_array_pub{nullptr}; - orb_advert_t _debug_key_value_pub{nullptr}; - orb_advert_t _debug_value_pub{nullptr}; - orb_advert_t _debug_vect_pub{nullptr}; - orb_advert_t _distance_sensor_pub{nullptr}; - orb_advert_t _flow_distance_sensor_pub{nullptr}; - orb_advert_t _flow_pub{nullptr}; - orb_advert_t _follow_target_pub{nullptr}; - orb_advert_t _global_pos_pub{nullptr}; - orb_advert_t _gps_inject_data_pub{nullptr}; - orb_advert_t _gps_pub{nullptr}; - orb_advert_t _gyro_pub{nullptr}; - orb_advert_t _hil_distance_sensor_pub{nullptr}; - orb_advert_t _land_detector_pub{nullptr}; - orb_advert_t _landing_target_pose_pub{nullptr}; - orb_advert_t _local_pos_pub{nullptr}; - orb_advert_t _mag_pub{nullptr}; - orb_advert_t _manual_pub{nullptr}; - orb_advert_t _mocap_odometry_pub{nullptr}; - orb_advert_t _obstacle_distance_pub{nullptr}; - orb_advert_t _offboard_control_mode_pub{nullptr}; - orb_advert_t _ping_pub{nullptr}; - orb_advert_t _pos_sp_triplet_pub{nullptr}; - orb_advert_t _radio_status_pub{nullptr}; - orb_advert_t _rates_sp_pub{nullptr}; - orb_advert_t _rc_pub{nullptr}; - orb_advert_t _trajectory_waypoint_pub{nullptr}; - orb_advert_t _transponder_report_pub{nullptr}; - orb_advert_t _visual_odometry_pub{nullptr}; - - uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - - static constexpr int _gps_inject_data_queue_size{6}; - - static constexpr unsigned int MOM_SWITCH_COUNT{8}; - - int _orb_class_instance{-1}; - - uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; - - uint16_t _mom_switch_state{0}; - - uint64_t _global_ref_timestamp{0}; - - float _hil_local_alt0{0.0f}; - - bool _hil_local_proj_inited{false}; - - uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; - - hrt_abstime _last_utm_global_pos_com{0}; + mavlink_status_t _status{}; ///< receiver status, used for mavlink_parse_char() + + // ORB publications + uORB::Publication _actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)}; + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + uORB::Publication _battery_pub{ORB_ID(battery_status)}; + uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; + uORB::Publication _debug_array_pub{ORB_ID(debug_array)}; + uORB::Publication _debug_key_value_pub{ORB_ID(debug_key_value)}; + uORB::Publication _debug_value_pub{ORB_ID(debug_value)}; + uORB::Publication _debug_vect_pub{ORB_ID(debug_vect)}; + uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; + uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; + uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; + uORB::Publication _flow_pub{ORB_ID(optical_flow)}; + uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; + uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _gps_pub{ORB_ID(vehicle_gps_position)}; + uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; + uORB::Publication _local_pos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; + uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; + uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + + // ORB publications (multi) + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; + uORB::PublicationMulti _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; + uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; + uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; + uORB::PublicationMulti _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW}; + uORB::PublicationMulti _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW}; + uORB::PublicationMulti _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW}; + uORB::PublicationMulti _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW}; + + // ORB publications (queue length > 1) + uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::PublicationQueued _transponder_report_pub{ORB_ID(transponder_report)}; + uORB::PublicationQueued _cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationQueued _cmd_pub{ORB_ID(vehicle_command)}; + + // ORB subscriptions + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _param_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + + static constexpr unsigned int MOM_SWITCH_COUNT{8}; + uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; + uint16_t _mom_switch_state{0}; + + uint64_t _global_ref_timestamp{0}; + + map_projection_reference_s _hil_local_proj_ref{}; + float _hil_local_alt0{0.0f}; + bool _hil_local_proj_inited{false}; + + hrt_abstime _last_utm_global_pos_com{0}; DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, (ParamFloat) _param_bat_low_thr, + (ParamInt) _param_com_flight_uuid, (ParamFloat) _param_sens_flow_maxhgt, (ParamFloat) _param_sens_flow_maxr, (ParamFloat) _param_sens_flow_minhgt, - (ParamInt) _param_com_flight_uuid, (ParamInt) _param_sens_flow_rot ); diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index abd12f621f07..48714ec16f4b 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -48,13 +48,6 @@ MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : { } -MavlinkTimesync::~MavlinkTimesync() -{ - if (_timesync_status_pub) { - orb_unadvertise(_timesync_status_pub); - } -} - void MavlinkTimesync::handle_message(const mavlink_message_t *msg) { @@ -145,7 +138,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) } // Publish status message - struct timesync_status_s tsync_status = {}; + timesync_status_s tsync_status{}; tsync_status.timestamp = hrt_absolute_time(); tsync_status.remote_timestamp = tsync.tc1 / 1000ULL; @@ -153,14 +146,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) tsync_status.estimated_offset = (int64_t)_time_offset; tsync_status.round_trip_time = rtt_us; - if (_timesync_status_pub == nullptr) { - int instance; - _timesync_status_pub = orb_advertise_multi(ORB_ID(timesync_status), &tsync_status, &instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(timesync_status), _timesync_status_pub, &tsync_status); - } - + _timesync_status_pub.publish(tsync_status); } break; diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h index 61758af56634..3204696f4260 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/modules/mavlink/mavlink_timesync.h @@ -42,7 +42,7 @@ #include "mavlink_bridge_header.h" -#include +#include #include #include @@ -98,7 +98,7 @@ class MavlinkTimesync { public: explicit MavlinkTimesync(Mavlink *mavlink); - ~MavlinkTimesync(); + ~MavlinkTimesync() = default; void handle_message(const mavlink_message_t *msg); @@ -132,7 +132,7 @@ class MavlinkTimesync */ void reset_filter(); - orb_advert_t _timesync_status_pub{nullptr}; + uORB::PublicationMulti _timesync_status_pub{ORB_ID(timesync_status)}; uint32_t _sequence{0}; diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index cd58851ed97a..cf251ed20a55 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -68,9 +68,6 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys MavlinkULog::~MavlinkULog() { - if (_ulog_stream_ack_pub) { - orb_unadvertise(_ulog_stream_ack_pub); - } } void MavlinkULog::start_ack_received() @@ -257,10 +254,5 @@ void MavlinkULog::publish_ack(uint16_t sequence) ack.timestamp = hrt_absolute_time(); ack.sequence = sequence; - if (_ulog_stream_ack_pub == nullptr) { - _ulog_stream_ack_pub = orb_advertise(ORB_ID(ulog_stream_ack), &ack); - - } else { - orb_publish(ORB_ID(ulog_stream_ack), _ulog_stream_ack_pub, &ack); - } + _ulog_stream_ack_pub.publish(ack); } diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h index 609f7d30fa68..f46775e8e64f 100644 --- a/src/modules/mavlink/mavlink_ulog.h +++ b/src/modules/mavlink/mavlink_ulog.h @@ -46,6 +46,7 @@ #include #include +#include #include #include #include @@ -121,7 +122,7 @@ class MavlinkULog static const float _rate_calculation_delta_t; ///< rate update interval uORB::SubscriptionData _ulog_stream_sub{ORB_ID(ulog_stream)}; - orb_advert_t _ulog_stream_ack_pub = nullptr; + uORB::Publication _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)}; uint16_t _wait_for_ack_sequence; uint8_t _sent_tries = 0; volatile bool _ack_received = false; ///< set to true if a matching ack received diff --git a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt index f4b1ace74ac9..84110b6d4e5b 100644 --- a/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt +++ b/src/modules/mc_att_control/AttitudeControl/CMakeLists.txt @@ -39,4 +39,4 @@ target_include_directories(AttitudeControl ${CMAKE_CURRENT_SOURCE_DIR} ) -px4_add_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) +px4_add_unit_gtest(SRC AttitudeControlTest.cpp LINKLIBS AttitudeControl) diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 3734e79b293c..82b8ad4e87dc 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -36,7 +36,6 @@ add_subdirectory(AttitudeControl) px4_add_module( MODULE modules__mc_att_control MAIN mc_att_control - STACK_MAIN 1200 STACK_MAX 3500 COMPILE_FLAGS SRCS @@ -46,4 +45,5 @@ px4_add_module( conversion mathlib AttitudeControl + px4_work_queue ) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index d6019cb2817d..153002b2cdd4 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -40,18 +40,18 @@ #include #include #include -#include +#include #include +#include #include +#include #include #include #include #include #include #include -#include -#include -#include +#include #include #include #include @@ -68,37 +68,36 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MAX_GYRO_COUNT 3 - - -class MulticopterAttitudeControl : public ModuleBase, public ModuleParams +class MulticopterAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem { public: MulticopterAttitudeControl(); - virtual ~MulticopterAttitudeControl() = default; + virtual ~MulticopterAttitudeControl(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static MulticopterAttitudeControl *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); private: /** * initialize some vectors/matrices from parameters */ - void parameters_updated(); + void parameters_updated(); /** * Check for parameter update and handle it. @@ -134,7 +133,7 @@ class MulticopterAttitudeControl : public ModuleBase /** * Attitude rates controller. */ - void control_attitude_rates(float dt); + void control_attitude_rates(float dt, const matrix::Vector3f &rates); /** * Throttle PID attenuation. @@ -152,17 +151,12 @@ class MulticopterAttitudeControl : public ModuleBase uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ - uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; - int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - unsigned _gyro_count{1}; - int _selected_gyro{0}; - - uORB::Publication _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ @@ -182,9 +176,6 @@ class MulticopterAttitudeControl : public ModuleBase struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct battery_status_s _battery_status {}; /**< battery status */ - struct sensor_gyro_s _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ - struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ - struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ struct vehicle_land_detected_s _vehicle_land_detected {}; struct landing_gear_s _landing_gear {}; @@ -204,11 +195,17 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _att_control; /**< attitude control vector */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + float _dt_accumulator{0.0f}; + int _loop_counter{0}; + + bool _reset_yaw_sp{true}; + float _attitude_dt{0.0f}; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_roll_p, (ParamFloat) _param_mc_rollrate_p, @@ -260,12 +257,6 @@ class MulticopterAttitudeControl : public ModuleBase (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off, - /* Stabilized mode params */ (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index e1e79c887aea..659b662a2ec7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,12 +63,9 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl() : ModuleParams(nullptr), + WorkItem(px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { - _sensor_gyro_sub[i] = -1; - } - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; /* initialize quaternions in messages to be valid */ @@ -82,15 +79,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _thrust_sp = 0.0f; _att_control.zero(); - /* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ - for (unsigned i = 0; i < 3; i++) { - // used scale factors to unity - _sensor_correction.gyro_scale_0[i] = 1.0f; - _sensor_correction.gyro_scale_1[i] = 1.0f; - _sensor_correction.gyro_scale_2[i] = 1.0f; + parameters_updated(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterAttitudeControl::init() +{ + if (!_vehicle_angular_velocity_sub.register_callback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; } - parameters_updated(); + return true; } void @@ -127,16 +132,6 @@ MulticopterAttitudeControl::parameters_updated() _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - - /* get transformation matrix from sensor/board to body frame */ - _board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); - - /* fine tune the rotation */ - Dcmf board_rotation_offset(Eulerf( - M_DEG_TO_RAD_F * _param_sens_board_x_off.get(), - M_DEG_TO_RAD_F * _param_sens_board_y_off.get(), - M_DEG_TO_RAD_F * _param_sens_board_z_off.get())); - _board_rotation = board_rotation_offset * _board_rotation; } void @@ -403,45 +398,13 @@ MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rat * Output: '_att_control' vector */ void -MulticopterAttitudeControl::control_attitude_rates(float dt) +MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates) { /* reset integral if disarmed */ if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _rates_int.zero(); } - // get the raw gyro data and correct for thermal errors - Vector3f rates; - - if (_selected_gyro == 0) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; - - } else if (_selected_gyro == 1) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; - - } else if (_selected_gyro == 2) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; - - } else { - rates(0) = _sensor_gyro.x; - rates(1) = _sensor_gyro.y; - rates(2) = _sensor_gyro.z; - } - - // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; - - // correct for in-run bias errors - rates(0) -= _sensor_bias.gyro_x_bias; - rates(1) -= _sensor_bias.gyro_y_bias; - rates(2) -= _sensor_bias.gyro_z_bias; - Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_param_mc_tpa_break_p.get(), _param_mc_tpa_rate_p.get())); Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_param_mc_tpa_break_i.get(), _param_mc_tpa_rate_i.get())); Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_param_mc_tpa_break_d.get(), _param_mc_tpa_rate_d.get())); @@ -509,7 +472,6 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* explicitly limit the integrator state */ for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { _rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i)); - } } @@ -532,9 +494,6 @@ MulticopterAttitudeControl::publish_rate_controller_status() { rate_ctrl_status_s rate_ctrl_status = {}; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed = _rates_prev(0); - rate_ctrl_status.pitchspeed = _rates_prev(1); - rate_ctrl_status.yawspeed = _rates_prev(2); rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); @@ -550,8 +509,8 @@ MulticopterAttitudeControl::publish_actuator_controls() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _sensor_gyro.timestamp; /* scale effort by battery status */ if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { @@ -566,222 +525,190 @@ MulticopterAttitudeControl::publish_actuator_controls() } void -MulticopterAttitudeControl::run() +MulticopterAttitudeControl::Run() { - _gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT); - - for (unsigned s = 0; s < _gyro_count; s++) { - _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + if (should_exit()) { + _vehicle_angular_velocity_sub.unregister_callback(); + exit_and_cleanup(); + return; } - /* wakeup source: gyro data from sensor selected by the sensor app */ - px4_pollfd_struct_t poll_fds = {}; - poll_fds.events = POLLIN; + perf_begin(_loop_perf); - const hrt_abstime task_start = hrt_absolute_time(); - hrt_abstime last_run = task_start; - float dt_accumulator = 0.f; - int loop_counter = 0; + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; - bool reset_yaw_sp = true; - float attitude_dt = 0.f; + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); - while (!should_exit()) { + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; - // check if the selected gyro has updated first - _sensor_correction_sub.update(&_sensor_correction); + const Vector3f rates{angular_velocity.xyz}; - /* update the latest gyro selection */ - if (_sensor_correction.selected_gyro_instance < _gyro_count) { - _selected_gyro = _sensor_correction.selected_gyro_instance; - } - - poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; + _actuators.timestamp_sample = angular_velocity.timestamp_sample; - /* wait for up to 100ms for data */ - int pret = px4_poll(&poll_fds, 1, 100); + /* run the rate controller immediately after a gyro update */ + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt, rates); - /* timed out - periodic check for should_exit() */ - if (pret == 0) { - continue; + publish_actuator_controls(); + publish_rate_controller_status(); } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_ERR("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - px4_usleep(100000); - continue; + /* check for updates in other topics */ + _v_control_mode_sub.update(&_v_control_mode); + _battery_status_sub.update(&_battery_status); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _landing_gear_sub.update(&_landing_gear); + vehicle_status_poll(); + vehicle_motor_limits_poll(); + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + const bool attitude_updated = vehicle_attitude_poll(); + + _attitude_dt += dt; + + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ + if (_v_control_mode.flag_control_rattitude_enabled) { + _v_control_mode.flag_control_attitude_enabled = + fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && + fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); } - perf_begin(_loop_perf); + bool attitude_setpoint_generated = false; - /* run controller on gyro changes */ - if (poll_fds.revents & POLLIN) { - const hrt_abstime now = hrt_absolute_time(); + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - last_run) / 1e6f), 0.0002f, 0.02f); - last_run = now; + // vehicle is a tailsitter in transition mode + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; - /* copy gyro data */ - orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro); + bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); - /* run the rate controller immediately after a gyro update */ - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); - - publish_actuator_controls(); - publish_rate_controller_status(); - } - - /* check for updates in other topics */ - _v_control_mode_sub.update(&_v_control_mode); - _battery_status_sub.update(&_battery_status); - _sensor_bias_sub.update(&_sensor_bias); - _vehicle_land_detected_sub.update(&_vehicle_land_detected); - _landing_gear_sub.update(&_landing_gear); - vehicle_status_poll(); - vehicle_motor_limits_poll(); - const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); - const bool attitude_updated = vehicle_attitude_poll(); - - attitude_dt += dt; - - /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't - * even bother running the attitude controllers */ - if (_v_control_mode.flag_control_rattitude_enabled) { - _v_control_mode.flag_control_attitude_enabled = - fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && - fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); - } - - bool attitude_setpoint_generated = false; - - const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_vehicle_status.in_transition_mode; - - // vehicle is a tailsitter in transition mode - const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; - - bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + if (run_att_ctrl) { + if (attitude_updated) { + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(_attitude_dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + } - if (run_att_ctrl) { - if (attitude_updated) { - // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode - if (_v_control_mode.flag_control_manual_enabled && - !_v_control_mode.flag_control_altitude_enabled && - !_v_control_mode.flag_control_velocity_enabled && - !_v_control_mode.flag_control_position_enabled) { - generate_attitude_setpoint(attitude_dt, reset_yaw_sp); - attitude_setpoint_generated = true; - } + control_attitude(); - control_attitude(); + if (_v_control_mode.flag_control_yawrate_override_enabled) { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + _v_rates_sp_sub.update(&_v_rates_sp); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; + } - if (_v_control_mode.flag_control_yawrate_override_enabled) { - /* Yaw rate override enabled, overwrite the yaw setpoint */ - _v_rates_sp_sub.update(&_v_rates_sp); - const auto yawrate_reference = _v_rates_sp.yaw; - _rates_sp(2) = yawrate_reference; - } + publish_rates_setpoint(); + } + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled && is_hovering) { + if (manual_control_updated) { + /* manual rates control - ACRO mode */ + Vector3f man_rate_sp( + math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); + _rates_sp = man_rate_sp.emult(_acro_rate_max); + _thrust_sp = _manual_control_sp.z; publish_rates_setpoint(); } } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled && is_hovering) { - - if (manual_control_updated) { - /* manual rates control - ACRO mode */ - Vector3f man_rate_sp( - math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); - _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = _manual_control_sp.z; - publish_rates_setpoint(); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_rates_sp_sub.update(&_v_rates_sp)) { - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = -_v_rates_sp.thrust_body[2]; - } + if (_v_rates_sp_sub.update(&_v_rates_sp)) { + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = -_v_rates_sp.thrust_body[2]; } } + } - if (_v_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - publish_actuator_controls(); - } + if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + publish_actuator_controls(); } + } - if (attitude_updated) { - // reset yaw setpoint during transitions, tailsitter.cpp generates - // attitude setpoint for the transition - reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || - _vehicle_land_detected.landed || - (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); + if (attitude_updated) { + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || + _vehicle_land_detected.landed || + (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); - attitude_dt = 0.f; - } + _attitude_dt = 0.f; + } - /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ - if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { - dt_accumulator += dt; - ++loop_counter; - - if (dt_accumulator > 1.f) { - const float loop_update_rate = (float)loop_counter / dt_accumulator; - _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; - dt_accumulator = 0; - loop_counter = 0; - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); - } + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ + if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { + _dt_accumulator += dt; + ++_loop_counter; + + if (_dt_accumulator > 1.f) { + const float loop_update_rate = (float)_loop_counter / _dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + _dt_accumulator = 0; + _loop_counter = 0; + _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); } - - parameter_update_poll(); } - perf_end(_loop_perf); + parameter_update_poll(); } - for (unsigned s = 0; s < _gyro_count; s++) { - orb_unsubscribe(_sensor_gyro_sub[s]); - } + perf_end(_loop_perf); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL, - 1700, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; + MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - return 0; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } -MulticopterAttitudeControl *MulticopterAttitudeControl::instantiate(int argc, char *argv[]) +int MulticopterAttitudeControl::print_status() { - return new MulticopterAttitudeControl(); + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + print_message(_actuators); + + return 0; } int MulticopterAttitudeControl::custom_command(int argc, char *argv[]) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 52304cdc47d4..0cafa88fcf6c 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -609,21 +609,3 @@ PARAM_DEFINE_FLOAT(MC_TPA_RATE_D, 0.0f); */ PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); -/** - * Multicopter air-mode - * - * The air-mode enables the mixer to increase the total thrust of the multirotor - * in order to keep attitude and rate control even at low and high throttle. - * - * This function should be disabled during tuning as it will help the controller - * to diverge if the closed-loop is unstable (i.e. the vehicle is not tuned yet). - * - * Enabling air-mode for yaw requires the use of an arming switch. - * - * @value 0 Disabled - * @value 1 Roll/Pitch - * @value 2 Roll/Pitch/Yaw - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_INT32(MC_AIRMODE, 0); - diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index b7654d6a748b..6a75028b0106 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -37,7 +37,7 @@ px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control COMPILE_FLAGS - STACK_MAIN 1300 + -Wno-implicit-fallthrough # TODO: fix and remove SRCS mc_pos_control_main.cpp PositionControl.cpp diff --git a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt index 164b175ed979..9c78d9f8cd94 100644 --- a/src/modules/mc_pos_control/Takeoff/CMakeLists.txt +++ b/src/modules/mc_pos_control/Takeoff/CMakeLists.txt @@ -39,4 +39,4 @@ target_include_directories(Takeoff ${CMAKE_CURRENT_SOURCE_DIR} ) -px4_add_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) +px4_add_unit_gtest(SRC TakeoffTest.cpp LINKLIBS Takeoff) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 944b4cd0788f..d3dd8b3fc26c 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -58,6 +58,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool break; } + // FALLTHROUGH case TakeoffState::spoolup: if (_spoolup_time_hysteresis.get_state()) { _takeoff_state = TakeoffState::ready_for_takeoff; @@ -66,6 +67,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool break; } + // FALLTHROUGH case TakeoffState::ready_for_takeoff: if (want_takeoff) { _takeoff_state = TakeoffState::rampup; @@ -75,6 +77,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool break; } + // FALLTHROUGH case TakeoffState::rampup: if (_takeoff_ramp_vz >= takeoff_desired_vz) { _takeoff_state = TakeoffState::flight; @@ -83,6 +86,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool break; } + // FALLTHROUGH case TakeoffState::flight: if (landed) { _takeoff_state = TakeoffState::ready_for_takeoff; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d2c7cfb30038..8ee29807494f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -108,7 +109,7 @@ class MulticopterPositionControl : public ModuleBase Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */ + uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ orb_advert_t _mavlink_log_pub{nullptr}; orb_id_t _attitude_setpoint_id{nullptr}; @@ -322,13 +323,13 @@ MulticopterPositionControl::parameters_update(bool force) if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { _param_mpc_xy_cruise.set(_param_mpc_xy_vel_max.get()); _param_mpc_xy_cruise.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed") + mavlink_log_critical(&_mavlink_log_pub, "Cruise speed has been constrained by max speed"); } if (_param_mpc_vel_manual.get() > _param_mpc_xy_vel_max.get()) { _param_mpc_vel_manual.set(_param_mpc_xy_vel_max.get()); _param_mpc_vel_manual.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed") + mavlink_log_critical(&_mavlink_log_pub, "Manual speed has been constrained by max speed"); } if (_param_mpc_thr_hover.get() > _param_mpc_thr_max.get() || @@ -336,7 +337,7 @@ MulticopterPositionControl::parameters_update(bool force) _param_mpc_thr_hover.set(math::constrain(_param_mpc_thr_hover.get(), _param_mpc_thr_min.get(), _param_mpc_thr_max.get())); _param_mpc_thr_hover.commit(); - mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max") + mavlink_log_critical(&_mavlink_log_pub, "Hover thrust has been constrained by min/max"); } _flight_tasks.handleParameterUpdate(); @@ -534,12 +535,14 @@ MulticopterPositionControl::run() if (_wv_controller != nullptr) { // in manual mode we just want to use weathervane if position is controlled as well - if (_wv_controller->weathervane_enabled() && (!_control_mode.flag_control_manual_enabled - || _control_mode.flag_control_position_enabled)) { - _wv_controller->activate(); + // in mission, enabling wv is done in flight task + if (_control_mode.flag_control_manual_enabled) { + if (_control_mode.flag_control_position_enabled && _wv_controller->weathervane_enabled()) { + _wv_controller->activate(); - } else { - _wv_controller->deactivate(); + } else { + _wv_controller->deactivate(); + } } _wv_controller->update(matrix::Quatf(_att_sp.q_d), _states.yaw); @@ -736,9 +739,9 @@ MulticopterPositionControl::start_flight_task() if (_vehicle_status.in_transition_mode) { should_disable_task = false; - int error = _flight_tasks.switchTask(FlightTaskIndex::Transition); + FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Transition); - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Transition activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -763,9 +766,9 @@ MulticopterPositionControl::start_flight_task() _control_mode.flag_control_acceleration_enabled)) { should_disable_task = false; - int error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); + FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Offboard); - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Offboard activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -782,9 +785,9 @@ MulticopterPositionControl::start_flight_task() // Auto-follow me if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { should_disable_task = false; - int error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); + FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe); - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Follow-Me activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -800,7 +803,7 @@ MulticopterPositionControl::start_flight_task() } else if (_control_mode.flag_control_auto_enabled) { // Auto related maneuvers should_disable_task = false; - int error = 0; + FlightTaskError error = FlightTaskError::NoError; switch (_param_mpc_auto_mode.get()) { case 1: @@ -812,7 +815,7 @@ MulticopterPositionControl::start_flight_task() break; } - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Auto activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -829,7 +832,7 @@ MulticopterPositionControl::start_flight_task() // manual position control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { should_disable_task = false; - int error = 0; + FlightTaskError error = FlightTaskError::NoError; switch (_param_mpc_pos_mode.get()) { case 1: @@ -849,7 +852,7 @@ MulticopterPositionControl::start_flight_task() break; } - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Position-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -866,7 +869,7 @@ MulticopterPositionControl::start_flight_task() // manual altitude control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || task_failure) { should_disable_task = false; - int error = 0; + FlightTaskError error = FlightTaskError::NoError; switch (_param_mpc_pos_mode.get()) { case 1: @@ -882,7 +885,7 @@ MulticopterPositionControl::start_flight_task() break; } - if (error != 0) { + if (error != FlightTaskError::NoError) { if (prev_failure_count == 0) { PX4_WARN("Altitude-Ctrl activation failed with error: %s", _flight_tasks.errorToString(error)); } @@ -905,9 +908,9 @@ MulticopterPositionControl::start_flight_task() // for some reason no flighttask was able to start. // go into failsafe flighttask - int error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe); + FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::Failsafe); - if (error != 0) { + if (error != FlightTaskError::NoError) { // No task was activated. _flight_tasks.switchTask(FlightTaskIndex::None); } @@ -1038,13 +1041,7 @@ void MulticopterPositionControl::send_vehicle_cmd_do(uint8_t nav_state) } // publish the vehicle command - if (_pub_vehicle_command == nullptr) { - _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, - vehicle_command_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); - } + _pub_vehicle_command.publish(command); } int MulticopterPositionControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index c6f454aa3c78..7a9871f43334 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -253,17 +253,17 @@ PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); * Proportional gain for horizontal trajectory position error * * @min 0.1 - * @max 5.0 + * @max 1.0 * @decimal 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.3f); +PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); /** * Proportional gain for vertical trajectory position error * * @min 0.1 - * @max 5.0 + * @max 1.0 * @decimal 1 * @group Multicopter Position Control */ diff --git a/src/modules/muorb/adsp/px4muorb.cpp b/src/modules/muorb/adsp/px4muorb.cpp index 6b27cb742f6b..b791ebb9830c 100644 --- a/src/modules/muorb/adsp/px4muorb.cpp +++ b/src/modules/muorb/adsp/px4muorb.cpp @@ -35,7 +35,6 @@ #include "uORBFastRpcChannel.hpp" #include "uORBManager.hpp" -#include #include #include #include diff --git a/src/modules/muorb/test/muorb_test_example.cpp b/src/modules/muorb/test/muorb_test_example.cpp index d8af5f81e539..364b24981f03 100644 --- a/src/modules/muorb/test/muorb_test_example.cpp +++ b/src/modules/muorb/test/muorb_test_example.cpp @@ -43,6 +43,9 @@ #include #include #include +#include +#include +#include px4::AppState MuorbTestExample::appState; @@ -50,7 +53,7 @@ int MuorbTestExample::main() { int rc; appState.setRunning(true); - rc = PingPongTest(); + rc = PingPongTest(); appState.setRunning(false); return rc; } @@ -58,63 +61,37 @@ int MuorbTestExample::main() int MuorbTestExample::DefaultTest() { int i = 0; - orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status); - if (pub_id == nullptr) { - PX4_ERR("error publishing esc_status"); - return -1; - } - - orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - - if (pub_id_vc == nullptr) { - PX4_ERR("error publishing vehicle_command"); - return -1; - } - - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - return -1; - } - - int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); - - if (sub_vc == PX4_ERROR) { - PX4_ERR("Error subscribing to vehicle_command topic"); - return -1; - } + uORB::Subscription sub_vc{ORB_ID(vehicle_command)}; + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Publication pub_id{ORB_ID(esc_status)}; + pub_id.publish(m_esc_status); while (!appState.exitRequested() && i < 100) { PX4_DEBUG("[%d] Doing work...", i); - if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) { + if (!pub_id.publish(m_esc_status)) { PX4_ERR("[%d]Error publishing the esc status message for iter", i); break; } - bool updated = false; - - if (orb_check(sub_vc, &updated) == 0) { - if (updated) { - PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); - - if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { - PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); - break; - } + if (sub_vc.updated()) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - break; - } + if (!sub_vc.copy(&m_vc)) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } - } else { - PX4_DEBUG("[%d] VC topic is not updated", i); + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; } } else { PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + PX4_DEBUG("[%d] VC topic is not updated", i); break; } @@ -127,51 +104,28 @@ int MuorbTestExample::DefaultTest() int MuorbTestExample::PingPongTest() { int i = 0; - orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - - if (pub_id_vc == nullptr) { - PX4_ERR("error publishing vehicle_command"); - return -1; - } - - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - return -1; - } - - int sub_esc_status = orb_subscribe(ORB_ID(esc_status)); - - if (sub_esc_status == PX4_ERROR) { - PX4_ERR("Error subscribing to esc_status topic"); - return -1; - } + uORB::PublicationQueued vcmd_pub{ORB_ID(vehicle_command)}; + uORB::Subscription sub_esc_status{ORB_ID(esc_status)}; while (!appState.exitRequested()) { PX4_INFO("[%d] Doing work...", i); - bool updated = false; - - if (orb_check(sub_esc_status, &updated) == 0) { - if (updated) { - PX4_INFO("[%d]ESC status is updated... reading new value", i); - if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) { - PX4_ERR("[%d]Error calling orb copy for esc status... ", i); - break; - } + if (sub_esc_status.updated()) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); - if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the vechile command message", i); - break; - } + if (!sub_esc_status.copy(&m_esc_status)) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } - } else { - PX4_INFO("[%d] esc status topic is not updated", i); + if (!vcmd_pub.publish(m_vc)) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; } } else { - PX4_ERR("[%d]Error checking the updated status for esc status... ", i); - break; + PX4_INFO("[%d] esc status topic is not updated", i); } // sleep for 1 sec. diff --git a/src/modules/muorb/test/muorb_test_main.cpp b/src/modules/muorb/test/muorb_test_main.cpp index 2d83e8e20174..995cc49da1f1 100644 --- a/src/modules/muorb/test/muorb_test_main.cpp +++ b/src/modules/muorb/test/muorb_test_main.cpp @@ -37,9 +37,9 @@ * * @author Mark Charlebois */ -#include #include #include +#include #include "muorb_test_example.h" #include #include "uORB/uORBManager.hpp" diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index a11c4de27e4f..61d846de4a61 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -34,8 +34,6 @@ px4_add_module( MODULE modules__navigator MAIN navigator - STACK_MAIN 1300 - COMPILE_FLAGS SRCS navigator_main.cpp navigator_mode.cpp diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 47b1d0b7656a..faa3a3065a64 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -673,6 +673,9 @@ Mission::set_mission_items() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + // allow weather vane in mission + pos_sp_triplet->current.allow_weather_vane = true; + /* do takeoff before going to setpoint if needed and not already in takeoff */ /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ if (do_need_vertical_takeoff() && @@ -752,6 +755,29 @@ Mission::set_mission_items() _work_item_type == WORK_ITEM_TYPE_TAKEOFF && new_work_item_type == WORK_ITEM_TYPE_DEFAULT && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_navigator->get_land_detected()->landed) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.allow_weather_vane = false; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WORK_ITEM_TYPE_ALIGN && + _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_navigator->get_land_detected()->landed) { /* check if the vtol_takeoff waypoint is on top of us */ @@ -934,6 +960,9 @@ Mission::set_mission_items() && !_navigator->get_land_detected()->landed && has_next_position_item) { + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.allow_weather_vane = false; + new_work_item_type = WORK_ITEM_TYPE_ALIGN; set_align_mission_item(&_mission_item, &mission_item_next_position); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index fa459319a39d..efbf8faec82a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -319,7 +319,7 @@ MissionBlock::is_mission_item_reached() } } - if (_waypoint_position_reached) { + if (_waypoint_position_reached && !_waypoint_position_reached_previously) { // reached just now _time_wp_reached = now; } @@ -408,6 +408,7 @@ MissionBlock::is_mission_item_reached() } // all acceptance criteria must be met in the same iteration + _waypoint_position_reached_previously = _waypoint_position_reached; _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ac342b65e9bb..e6ca6a5febc7 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -129,6 +129,7 @@ class MissionBlock : public NavigatorMode bool _waypoint_position_reached{false}; bool _waypoint_yaw_reached{false}; + bool _waypoint_position_reached_previously{false}; hrt_abstime _time_first_inside_orbit{0}; hrt_abstime _action_start{0}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 7643b2cad24e..b0549c8a28cb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -330,8 +331,9 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ - orb_advert_t _vehicle_cmd_ack_pub{nullptr}; - orb_advert_t _vehicle_cmd_pub{nullptr}; + + uORB::PublicationQueued _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationQueued _vehicle_cmd_pub{ORB_ID(vehicle_command)}; // Subscriptions home_position_s _home_pos{}; /**< home position for RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8aae5510aede..3f9b6336ebc1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -903,7 +903,7 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; - transponder_report_s tr = {}; + transponder_report_s tr{}; tr.timestamp = hrt_absolute_time(); tr.icao_address = 1234; tr.lat = lat; // Latitude, expressed as degrees @@ -923,8 +923,8 @@ void Navigator::fake_traffic(const char *callsign, float distance, float directi transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields tr.squawk = 6667; - orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(h); + uORB::PublicationQueued tr_pub{ORB_ID(transponder_report)}; + tr_pub.publish(tr); } void Navigator::check_traffic() @@ -1142,12 +1142,7 @@ Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) break; } - if (_vehicle_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, vcmd); - - } else { - _vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - } + _vehicle_cmd_pub.publish(*vcmd); } void @@ -1165,13 +1160,7 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res command_ack.result_param1 = 0; command_ack.result_param2 = 0; - if (_vehicle_cmd_ack_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command_ack), _vehicle_cmd_ack_pub, &command_ack); - - } else { - _vehicle_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - } + _vehicle_cmd_ack_pub.publish(command_ack); } int Navigator::print_usage(const char *reason) diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index b3a37b62203a..be337d0858e0 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -45,11 +45,12 @@ add_library(px4iofirmware set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES px4iofirmware) target_link_libraries(px4iofirmware PUBLIC + arch_io_pins nuttx_apps nuttx_arch nuttx_c mixer rc perf - pwm_limit -) \ No newline at end of file + output_limit +) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index dc30e3b606dd..80943a7b737e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include @@ -211,9 +211,12 @@ mixer_tick(void) ); should_arm_nothrottle = ( - (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ - && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) /* and there is valid input via or mixer */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */ + && (((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_PREARMED) /* and FMU is prearmed */ + && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */ + || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */ + ) ); should_always_enable_pwm = ( @@ -311,8 +314,8 @@ mixer_tick(void) mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits); /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, - r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + output_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { @@ -412,7 +415,14 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { - control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + if (r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)] == INT16_MAX) { + //catch NAN values encoded as INT16 max for disarmed outputs + control = NAN; + + } else { + control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + } + break; } @@ -475,7 +485,7 @@ mixer_callback(uintptr_t handle, } /* motor spinup phase - lock throttle to zero */ - if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { + if ((pwm_limit.state == OUTPUT_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if ((control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE || control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) && control_index == actuator_controls_s::INDEX_THROTTLE) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 08092bd3382e..3dde775ef617 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -182,15 +182,16 @@ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ -#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ -#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ -#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ -#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ -#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ -#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ -#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ -#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ -#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ +#define PX4IO_P_SETUP_ARMING_FMU_PREARMED (1 << 2) /* FMU is already prearmed */ +#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 3) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 4) /* use custom failsafe values, not 0 values of mixer */ +#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 5) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 6) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 7) /* Disable the IO-internal evaluation of the RC */ +#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 8) /* If set, the system operates normally, but won't actuate any servos */ +#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 9) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 10) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 11) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 54e9ba12e388..427fc4e7ec52 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -55,7 +55,7 @@ #include #include -#include +#include #include @@ -68,7 +68,7 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -pwm_limit_t pwm_limit; +output_limit_t pwm_limit; float dt; @@ -338,7 +338,7 @@ user_start(int argc, char *argv[]) syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); /* initialize PWM limit lib */ - pwm_limit_init(&pwm_limit); + output_limit_init(&pwm_limit); /* * P O L I C E L I G H T S diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 0b3d0634d770..e5c21d0fa43e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -50,7 +50,7 @@ #include "protocol.h" -#include +#include /* hotfix: we are critically short of memory in px4io and this is the @@ -160,7 +160,7 @@ extern bool update_trims; /* * PWM limit structure */ -extern pwm_limit_t pwm_limit; +extern output_limit_t pwm_limit; /* * GPIO handling. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cd9d909e115..1e43a5fb80ed 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,6 +191,7 @@ volatile uint16_t r_page_setup[] = { PX4IO_P_SETUP_FEATURES_PWM_RSSI) #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ + PX4IO_P_SETUP_ARMING_FMU_PREARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ diff --git a/src/modules/replay/CMakeLists.txt b/src/modules/replay/CMakeLists.txt index 53655e29744d..d93d25491536 100644 --- a/src/modules/replay/CMakeLists.txt +++ b/src/modules/replay/CMakeLists.txt @@ -34,8 +34,6 @@ px4_add_module( MODULE modules__replay MAIN replay COMPILE_FLAGS - STACK_MAIN 1000 - STACK_MAX 4000 SRCS replay_main.cpp DEPENDS diff --git a/src/modules/replay/replay_main.cpp b/src/modules/replay/replay_main.cpp index b141e4c5059e..e1f897b3d54c 100644 --- a/src/modules/replay/replay_main.cpp +++ b/src/modules/replay/replay_main.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -847,7 +848,9 @@ void Replay::run() PX4_INFO("Replay done (published %u msgs, %.3lf s)", nr_published_messages, (double)hrt_elapsed_time(&_replay_start_time) / 1.e6); - //TODO: should we close the log file & exit (optionally, by adding a parameter -q) ? + //TODO: add parameter -q? + replay_file.close(); + px4_shutdown_request(false, false); } onExitMainLoop(); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index f35a72906f01..5f100e970393 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -61,10 +61,8 @@ extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]); RoverPositionControl::RoverPositionControl() : ModuleParams(nullptr), /* performance counters */ - _sub_sensors(ORB_ID(sensor_bias)), _loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters { - } RoverPositionControl::~RoverPositionControl() @@ -158,7 +156,8 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, bool setpoint = true; - if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + if ((_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) { /* AUTONOMOUS FLIGHT */ _control_mode_current = UGV_POSCTRL_MODE_AUTO; @@ -197,7 +196,7 @@ RoverPositionControl::control_position(const matrix::Vector2f ¤t_position, const Vector3f vel = R_to_body * Vector3f(ground_speed(0), ground_speed(1), ground_speed(2)); const float x_vel = vel(0); - const float x_acc = _sub_sensors.get().accel_x; + const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; // Compute airspeed control out and just scale it as a constant mission_throttle = _param_throttle_speed_scaler.get() @@ -274,6 +273,7 @@ RoverPositionControl::run() { _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); @@ -285,6 +285,7 @@ RoverPositionControl::run() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_local_pos_sub, 20); parameters_update(_params_sub, true); @@ -314,7 +315,7 @@ RoverPositionControl::run() vehicle_control_mode_poll(); //manual_control_setpoint_poll(); - _sub_sensors.update(); + _vehicle_acceleration_sub.update(); /* update parameters from storage */ parameters_update(_params_sub); @@ -327,10 +328,24 @@ RoverPositionControl::run() /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); position_setpoint_triplet_poll(); vehicle_attitude_poll(); + //Convert Local setpoints to global setpoints + if (_control_mode.flag_control_offboard_enabled) { + if (!globallocalconverter_initialized()) { + globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon, + _local_pos.ref_alt, _local_pos.ref_timestamp); + + } else { + globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z, + &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt); + + } + } + // update the reset counters in any case _pos_reset_counter = _global_pos.lat_lon_reset_counter; @@ -413,6 +428,7 @@ RoverPositionControl::run() orb_unsubscribe(_control_mode_sub); orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_local_pos_sub); orb_unsubscribe(_manual_control_sub); orb_unsubscribe(_params_sub); orb_unsubscribe(_pos_sp_triplet_sub); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index fc14dfafa27f..c10a3cc68150 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -58,12 +58,13 @@ #include #include #include -#include #include +#include #include #include #include #include +#include #include #include #include @@ -102,6 +103,7 @@ class RoverPositionControl : public ModuleBase, public Mod int _control_mode_sub{-1}; /**< control mode subscription */ int _global_pos_sub{-1}; + int _local_pos_sub{-1}; int _manual_control_sub{-1}; /**< notification of manual control updates */ int _params_sub{-1}; /**< notification of parameter updates */ int _pos_sp_triplet_sub{-1}; @@ -112,11 +114,12 @@ class RoverPositionControl : public ModuleBase, public Mod position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ vehicle_control_mode_s _control_mode{}; /**< control mode */ vehicle_global_position_s _global_pos{}; /**< global vehicle position */ + vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; sensor_combined_s _sensor_combined{}; - SubscriptionData _sub_sensors; + SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index f3910e25ce6b..f9f51324f104 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,11 +31,13 @@ # ############################################################################ +add_subdirectory(vehicle_acceleration) +add_subdirectory(vehicle_angular_velocity) + px4_add_module( MODULE modules__sensors MAIN sensors PRIORITY "SCHED_PRIORITY_MAX-5" - STACK_MAIN 1500 SRCS voted_sensors_update.cpp rc_update.cpp @@ -50,4 +52,6 @@ px4_add_module( git_ecl ecl_validation mathlib + sensors__vehicle_acceleration + sensors__vehicle_angular_velocity ) diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/sensors/rc_update.cpp index 9df56ac5af2a..bb8824a8ebbb 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/sensors/rc_update.cpp @@ -39,9 +39,6 @@ #include "rc_update.h" -#include -#include - using namespace sensors; RCUpdate::RCUpdate(const Parameters ¶meters) @@ -316,14 +313,13 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) _rc.frame_drop_count = rc_input.rc_lost_frame_count; /* publish rc_channels topic even if signal is invalid, for debug */ - int instance; - orb_publish_auto(ORB_ID(rc_channels), &_rc_pub, &_rc, &instance, ORB_PRIO_DEFAULT); + _rc_pub.publish(_rc); /* only publish manual control if the signal is still present and was present once */ if (!signal_lost && rc_input.timestamp_last_signal > 0) { /* initialize manual setpoint */ - struct manual_control_setpoint_s manual = {}; + manual_control_setpoint_s manual{}; /* set mode slot to unassigned */ manual.mode_slot = manual_control_setpoint_s::MODE_SLOT_NONE; /* set the timestamp to the last signal time */ @@ -405,11 +401,10 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) _parameters.rc_man_th, _parameters.rc_man_inv); /* publish manual_control_setpoint topic */ - orb_publish_auto(ORB_ID(manual_control_setpoint), &_manual_control_pub, &manual, &instance, - ORB_PRIO_HIGH); + _manual_control_pub.publish(manual); /* copy from mapped manual control to control group 3 */ - struct actuator_controls_s actuator_group_3 = {}; + actuator_controls_s actuator_group_3{}; actuator_group_3.timestamp = rc_input.timestamp_last_signal; @@ -423,8 +418,7 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) actuator_group_3.control[7] = manual.aux3; /* publish actuator_controls_3 topic */ - orb_publish_auto(ORB_ID(actuator_controls_3), &_actuator_group_3_pub, &actuator_group_3, &instance, - ORB_PRIO_DEFAULT); + _actuator_group_3_pub.publish(actuator_group_3); /* Update parameters from RC Channels (tuning with RC) if activated */ if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) { diff --git a/src/modules/sensors/rc_update.h b/src/modules/sensors/rc_update.h index 7e48a2a8f812..5f805919d814 100644 --- a/src/modules/sensors/rc_update.h +++ b/src/modules/sensors/rc_update.h @@ -44,7 +44,11 @@ #include #include #include +#include +#include #include +#include +#include #include #include @@ -105,9 +109,10 @@ class RCUpdate uORB::Subscription _rc_sub{ORB_ID(input_rc)}; /**< raw rc channels data subscription */ uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; /**< rc parameter map subscription */ - orb_advert_t _rc_pub = nullptr; /**< raw r/c control topic */ - orb_advert_t _manual_control_pub = nullptr; /**< manual control signal topic */ - orb_advert_t _actuator_group_3_pub = nullptr;/**< manual control as actuator topic */ + uORB::Publication _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ + uORB::Publication _actuator_group_3_pub{ORB_ID(actuator_controls_3)}; /**< manual control as actuator topic */ + + uORB::PublicationMulti _manual_control_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_HIGH}; /**< manual control signal topic */ rc_channels_s _rc {}; /**< r/c channel data */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 0c00e5c65642..f32238762922 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -115,6 +115,9 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); * @max 1500 * @group Sensors * @unit hPa + * + * @reboot_required true + * */ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); @@ -158,6 +161,7 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); * @value 32 Pitch 9°, Yaw 180° * @value 33 Pitch 45° * @value 34 Pitch 315° + * @value 35 Roll 90°, Yaw 270° * * @reboot_required true * @@ -210,9 +214,8 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); /** * Driver level cutoff frequency for gyro * -* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. This features -* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the -* controllers, not the estimators. 0 disables the filter. +* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. * * @min 0 * @max 1000 @@ -222,12 +225,31 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); */ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f); +/** +* Gyro control data maximum publication rate +* +* This is the maximum rate the gyro control data (sensor_gyro_control) will be allowed to publish at. +* Set to 0 to disable and publish at the native sensor sample rate. +* +* @min 0 +* @max 2000 +* @value 0 0 (no limit) +* @value 50 50 Hz +* @value 250 250 Hz +* @value 400 400 Hz +* @value 1000 1000 Hz +* @value 2000 2000 Hz +* @unit Hz +* @reboot_required true +* @group Sensors +*/ +PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0); + /** * Driver level cutoff frequency for accel * -* The cutoff frequency for the 2nd order butterworth filter on the accel driver. This features -* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the -* controllers, not the estimators. 0 disables the filter. +* The cutoff frequency for the 2nd order butterworth filter on the accel driver. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. * * @min 0 * @max 1000 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4f838af8514f..ca6f16f7adb6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,6 +92,9 @@ #include "rc_update.h" #include "voted_sensors_update.h" +#include "vehicle_acceleration/VehicleAcceleration.hpp" +#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" + using namespace DriverFramework; using namespace sensors; using namespace time_literals; @@ -135,7 +138,7 @@ class Sensors : public ModuleBase, public ModuleParams { public: Sensors(bool hil_enabled); - ~Sensors() = default; + ~Sensors() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -165,12 +168,14 @@ class Sensors : public ModuleBase, public ModuleParams uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */ uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; /**< raw differential pressure subscription */ - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - orb_advert_t _sensor_pub{nullptr}; /**< combined sensor data topic */ - orb_advert_t _airdata_pub{nullptr}; /**< combined sensor data topic */ - orb_advert_t _magnetometer_pub{nullptr}; /**< combined sensor data topic */ + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; /**< airspeed */ + uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; /**< combined sensor data topic */ + uORB::Publication _sensor_preflight{ORB_ID(sensor_preflight)}; /**< sensor preflight topic */ + uORB::Publication _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */ + uORB::Publication _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */ #if BOARD_NUMBER_BRICKS > 0 orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */ @@ -182,17 +187,13 @@ class Sensors : public ModuleBase, public ModuleParams int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */ #endif /* BOARD_NUMBER_BRICKS > 1 */ - orb_advert_t _airspeed_pub{nullptr}; /**< airspeed */ - orb_advert_t _sensor_preflight{nullptr}; /**< sensor preflight topic */ - perf_counter_t _loop_perf; /**< loop performance counter */ DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL differential_pressure_s _diff_pres {}; - - orb_advert_t _diff_pres_pub{nullptr}; /**< differential_pressure */ + uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */ #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ Parameters _parameters{}; /**< local copies of interesting parameters */ @@ -202,6 +203,10 @@ class Sensors : public ModuleBase, public ModuleParams VotedSensorsUpdate _voted_sensors_update; + VehicleAcceleration _vehicle_acceleration; + VehicleAngularVelocity _vehicle_angular_velocity; + + /** * Update our local parameter cache. */ @@ -253,6 +258,15 @@ Sensors::Sensors(bool hil_enabled) : } #endif /* BOARD_NUMBER_BRICKS > 0 */ + + _vehicle_acceleration.Start(); + _vehicle_angular_velocity.Start(); +} + +Sensors::~Sensors() +{ + _vehicle_acceleration.Stop(); + _vehicle_angular_velocity.Stop(); } int @@ -270,7 +284,7 @@ Sensors::parameters_update() } _rc_update.update_rc_functions(); - _voted_sensors_update.parameters_update(); + _voted_sensors_update.parametersUpdate(); return ret; } @@ -330,20 +344,19 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw) } /* don't risk to feed negative airspeed into the system */ - airspeed.indicated_airspeed_m_s = calc_indicated_airspeed_corrected((enum AIRSPEED_COMPENSATION_MODEL) + airspeed.indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL) _parameters.air_cmodel, smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, diff_pres.differential_pressure_filtered_pa, raw.baro_pressure_pa, air_temperature_celsius); - airspeed.true_airspeed_m_s = calc_true_airspeed_from_indicated(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, - air_temperature_celsius); + airspeed.true_airspeed_m_s = calc_TAS_from_EAS(airspeed.indicated_airspeed_m_s, raw.baro_pressure_pa, + air_temperature_celsius); // assume that EAS = IAS as we don't have an EAS-scale here airspeed.air_temperature_celsius = air_temperature_celsius; if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && PX4_ISFINITE(airspeed.true_airspeed_m_s)) { - int instance; - orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &airspeed, &instance, ORB_PRIO_DEFAULT); + _airspeed_pub.publish(airspeed); } } } @@ -459,8 +472,7 @@ Sensors::adc_poll() (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; - int instance; - orb_publish_auto(ORB_ID(differential_pressure), &_diff_pres_pub, &_diff_pres, &instance, ORB_PRIO_DEFAULT); + _diff_pres_pub.publish(_diff_pres); } } else @@ -573,14 +585,12 @@ Sensors::run() parameter_update_poll(true); /* get a set of initial values */ - _voted_sensors_update.sensors_poll(raw, airdata, magnetometer); + _voted_sensors_update.sensorsPoll(raw, airdata, magnetometer); diff_pres_poll(airdata); _rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */); - _sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt); - /* wakeup source */ px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; @@ -590,7 +600,7 @@ Sensors::run() while (!should_exit()) { /* use the best-voted gyro to pace output */ - poll_fds.fd = _voted_sensors_update.best_gyro_fd(); + poll_fds.fd = _voted_sensors_update.bestGyroFd(); /* wait for up to 50ms for data (Note that this implies, we can have a fail-over time of 50ms, * if a gyro fails) */ @@ -604,8 +614,8 @@ Sensors::run() /* if the polling operation failed because no gyro sensor is available yet, * then attempt to subscribe once again */ - if (_voted_sensors_update.num_gyros() == 0) { - _voted_sensors_update.initialize_sensors(); + if (_voted_sensors_update.numGyros() == 0) { + _voted_sensors_update.initializeSensors(); } px4_usleep(1000); @@ -621,12 +631,12 @@ Sensors::run() _armed = vcontrol_mode.flag_armed; } - /* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro + /* the timestamp of the raw struct is updated by the gyroPoll() method (this makes the gyro * a mandatory sensor) */ const uint64_t airdata_prev_timestamp = airdata.timestamp; const uint64_t magnetometer_prev_timestamp = magnetometer.timestamp; - _voted_sensors_update.sensors_poll(raw, airdata, magnetometer); + _voted_sensors_update.sensorsPoll(raw, airdata, magnetometer); /* check battery voltage */ adc_poll(); @@ -635,30 +645,30 @@ Sensors::run() if (raw.timestamp > 0) { - _voted_sensors_update.set_relative_timestamps(raw); + _voted_sensors_update.setRelativeTimestamps(raw); - int instance; - orb_publish_auto(ORB_ID(sensor_combined), &_sensor_pub, &raw, &instance, ORB_PRIO_DEFAULT); + _sensor_pub.publish(raw); if (airdata.timestamp != airdata_prev_timestamp) { - orb_publish_auto(ORB_ID(vehicle_air_data), &_airdata_pub, &airdata, &instance, ORB_PRIO_DEFAULT); + _airdata_pub.publish(airdata); } if (magnetometer.timestamp != magnetometer_prev_timestamp) { - orb_publish_auto(ORB_ID(vehicle_magnetometer), &_magnetometer_pub, &magnetometer, &instance, ORB_PRIO_DEFAULT); + _magnetometer_pub.publish(magnetometer); } - _voted_sensors_update.check_failover(); + _voted_sensors_update.checkFailover(); /* If the the vehicle is disarmed calculate the length of the maximum difference between * IMU units as a consistency metric and publish to the sensor preflight topic */ if (!_armed) { preflt.timestamp = hrt_absolute_time(); - _voted_sensors_update.calc_accel_inconsistency(preflt); - _voted_sensors_update.calc_gyro_inconsistency(preflt); - _voted_sensors_update.calc_mag_inconsistency(preflt); - orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt); + _voted_sensors_update.calcAccelInconsistency(preflt); + _voted_sensors_update.calcGyroInconsistency(preflt); + _voted_sensors_update.calcMagInconsistency(preflt); + + _sensor_preflight.publish(preflt); } } @@ -666,7 +676,7 @@ Sensors::run() * when not adding sensors poll for param updates */ if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) { - _voted_sensors_update.initialize_sensors(); + _voted_sensors_update.initializeSensors(); last_config_update = hrt_absolute_time(); } else { @@ -684,18 +694,6 @@ Sensors::run() perf_end(_loop_perf); } - if (_sensor_pub) { - orb_unadvertise(_sensor_pub); - } - - if (_airdata_pub) { - orb_unadvertise(_airdata_pub); - } - - if (_magnetometer_pub) { - orb_unadvertise(_magnetometer_pub); - } - _voted_sensors_update.deinit(); } @@ -719,11 +717,14 @@ int Sensors::task_spawn(int argc, char *argv[]) int Sensors::print_status() { - _voted_sensors_update.print_status(); + _voted_sensors_update.printStatus(); PX4_INFO("Airspeed status:"); _airspeed_validator.print(); + _vehicle_acceleration.PrintStatus(); + _vehicle_angular_velocity.PrintStatus(); + return 0; } diff --git a/src/modules/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt new file mode 100644 index 000000000000..337f659c46d1 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(sensors__vehicle_acceleration + VehicleAcceleration.cpp +) +target_link_libraries(sensors__vehicle_acceleration PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp new file mode 100644 index 000000000000..c20d1dc64630 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * 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 "VehicleAcceleration.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +VehicleAcceleration::VehicleAcceleration() : + ModuleParams(nullptr), + WorkItem(px4::wq_configurations::att_pos_ctrl), + _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")), + _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_acceleration: interval")), + _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency")) +{ +} + +VehicleAcceleration::~VehicleAcceleration() +{ + Stop(); + + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_sensor_latency_perf); +} + +bool +VehicleAcceleration::Start() +{ + // initialize thermal corrections as we might not immediately get a topic update (only non-zero values) + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset.zero(); + _bias.zero(); + + // force initial updates + ParametersUpdate(true); + SensorBiasUpdate(true); + + // needed to change the active sensor if the primary stops updating + _sensor_selection_sub.register_callback(); + + return SensorCorrectionsUpdate(true); +} + +void +VehicleAcceleration::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregister_callback(); + } + + _sensor_selection_sub.unregister_callback(); +} + +void +VehicleAcceleration::SensorBiasUpdate(bool force) +{ + if (_sensor_bias_sub.updated() || force) { + sensor_bias_s bias; + + if (_sensor_bias_sub.copy(&bias)) { + // TODO: should be checking device ID + _bias = Vector3f{bias.accel_bias}; + } + } +} + +bool +VehicleAcceleration::SensorCorrectionsUpdate(bool force) +{ + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + sensor_correction_s corrections{}; + _sensor_correction_sub.copy(&corrections); + + // TODO: should be checking device ID + if (_selected_sensor == 0) { + _offset = Vector3f{corrections.accel_offset_0}; + _scale = Vector3f{corrections.accel_scale_0}; + + } else if (_selected_sensor == 1) { + _offset = Vector3f{corrections.accel_offset_1}; + _scale = Vector3f{corrections.accel_scale_1}; + + } else if (_selected_sensor == 2) { + _offset = Vector3f{corrections.accel_offset_2}; + _scale = Vector3f{corrections.accel_scale_2}; + + } else { + _offset = Vector3f{0.0f, 0.0f, 0.0f}; + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + } + + // update the latest sensor selection + if ((_selected_sensor != corrections.selected_accel_instance) || force) { + if (corrections.selected_accel_instance < MAX_SENSOR_COUNT) { + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregister_callback(); + } + + const int sensor_new = corrections.selected_accel_instance; + + if (_sensor_sub[sensor_new].register_callback()) { + PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new); + _selected_sensor = sensor_new; + + return true; + } + } + } + } + + return false; +} + +void +VehicleAcceleration::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + // get transformation matrix from sensor/board to body frame + const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // fine tune the rotation + const Dcmf board_rotation_offset(Eulerf( + math::radians(_param_sens_board_x_off.get()), + math::radians(_param_sens_board_y_off.get()), + math::radians(_param_sens_board_z_off.get()))); + + _board_rotation = board_rotation_offset * board_rotation; + } +} + +void +VehicleAcceleration::Run() +{ + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // update corrections first to set _selected_sensor + SensorCorrectionsUpdate(); + + sensor_accel_s sensor_data; + + if (_sensor_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); + + ParametersUpdate(); + SensorBiasUpdate(); + + // get the sensor data and correct for thermal errors + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + + // apply offsets and scale + Vector3f accel{(val - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + accel = _board_rotation * accel; + + // correct for in-run bias errors + accel -= _bias; + + vehicle_acceleration_s out{}; + out.timestamp_sample = sensor_data.timestamp; + accel.copyTo(out.xyz); + out.timestamp = hrt_absolute_time(); + + _vehicle_acceleration_pub.publish(out); + } + + perf_end(_cycle_perf); +} + +void +VehicleAcceleration::PrintStatus() +{ + PX4_INFO("selected sensor: %d", _selected_sensor); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_sensor_latency_perf); +} diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp new file mode 100644 index 000000000000..f84cef18adf9 --- /dev/null +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +class VehicleAcceleration : public ModuleParams, public px4::WorkItem +{ +public: + + VehicleAcceleration(); + virtual ~VehicleAcceleration(); + + void Run() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + + void ParametersUpdate(bool force = false); + void SensorBiasUpdate(bool force = false); + bool SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_board_rot, + + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off + ) + + uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */ + {this, ORB_ID(sensor_accel), 0}, + {this, ORB_ID(sensor_accel), 1}, + {this, ORB_ID(sensor_accel), 2} + }; + + matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + + matrix::Vector3f _offset; + matrix::Vector3f _scale; + matrix::Vector3f _bias; + + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; + perf_counter_t _sensor_latency_perf; + + uint8_t _selected_sensor{0}; + +}; diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt new file mode 100644 index 000000000000..58aa0d92e3dd --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(sensors__vehicle_angular_velocity + VehicleAngularVelocity.cpp +) +target_link_libraries(sensors__vehicle_angular_velocity PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp new file mode 100644 index 000000000000..1471b646280e --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * 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 "VehicleAngularVelocity.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +VehicleAngularVelocity::VehicleAngularVelocity() : + ModuleParams(nullptr), + WorkItem(px4::wq_configurations::rate_ctrl), + _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), + _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")), + _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency")) +{ +} + +VehicleAngularVelocity::~VehicleAngularVelocity() +{ + Stop(); + + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_sensor_latency_perf); +} + +bool +VehicleAngularVelocity::Start() +{ + // initialize thermal corrections as we might not immediately get a topic update (only non-zero values) + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset.zero(); + _bias.zero(); + + // force initial updates + ParametersUpdate(true); + SensorBiasUpdate(true); + + // needed to change the active sensor if the primary stops updating + _sensor_selection_sub.register_callback(); + + return SensorCorrectionsUpdate(true); +} + +void +VehicleAngularVelocity::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_sub) { + sub.unregister_callback(); + } + + _sensor_selection_sub.unregister_callback(); +} + +void +VehicleAngularVelocity::SensorBiasUpdate(bool force) +{ + if (_sensor_bias_sub.updated() || force) { + sensor_bias_s bias; + + if (_sensor_bias_sub.copy(&bias)) { + // TODO: should be checking device ID + _bias = Vector3f{bias.gyro_bias}; + } + } +} + +bool +VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) +{ + if (_sensor_selection_sub.updated() || force) { + sensor_selection_s sensor_selection; + + if (_sensor_selection_sub.copy(&sensor_selection)) { + if (_selected_sensor_device_id != sensor_selection.gyro_device_id) { + _selected_sensor_device_id = sensor_selection.gyro_device_id; + force = true; + } + } + } + + // check if the selected sensor has updated + if (_sensor_correction_sub.updated() || force) { + + sensor_correction_s corrections{}; + _sensor_correction_sub.copy(&corrections); + + // TODO: should be checking device ID + if (_selected_sensor == 0) { + _offset = Vector3f{corrections.gyro_offset_0}; + _scale = Vector3f{corrections.gyro_scale_0}; + + } else if (_selected_sensor == 1) { + _offset = Vector3f{corrections.gyro_offset_1}; + _scale = Vector3f{corrections.gyro_scale_1}; + + } else if (_selected_sensor == 2) { + _offset = Vector3f{corrections.gyro_offset_2}; + _scale = Vector3f{corrections.gyro_scale_2}; + + } else { + _offset = Vector3f{0.0f, 0.0f, 0.0f}; + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + } + + // update the latest sensor selection + if ((_selected_sensor != corrections.selected_gyro_instance) || force) { + if (corrections.selected_gyro_instance < MAX_SENSOR_COUNT) { + // clear all registered callbacks + for (auto &sub : _sensor_control_sub) { + sub.unregister_callback(); + } + + for (auto &sub : _sensor_sub) { + sub.unregister_callback(); + } + + const int sensor_new = corrections.selected_gyro_instance; + + // subscribe to sensor_gyro_control if available + // currently not all drivers (eg df_*) provide sensor_gyro_control + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + sensor_gyro_control_s report{}; + _sensor_control_sub[i].copy(&report); + + if ((report.device_id != 0) && (report.device_id == _selected_sensor_device_id)) { + if (_sensor_control_sub[i].register_callback()) { + PX4_DEBUG("selected sensor (control) changed %d -> %d", _selected_sensor, i); + _selected_sensor_control = i; + + _sensor_control_available = true; + + // record selected sensor (sensor_gyro orb index) + _selected_sensor = sensor_new; + + return true; + } + } + } + + // otherwise fallback to using sensor_gyro (legacy that will be removed) + _sensor_control_available = false; + + if (_sensor_sub[sensor_new].register_callback()) { + PX4_DEBUG("selected sensor changed %d -> %d", _selected_sensor, sensor_new); + _selected_sensor = sensor_new; + + return true; + } + } + } + } + + return false; +} + +void +VehicleAngularVelocity::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + // get transformation matrix from sensor/board to body frame + const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // fine tune the rotation + const Dcmf board_rotation_offset(Eulerf( + math::radians(_param_sens_board_x_off.get()), + math::radians(_param_sens_board_y_off.get()), + math::radians(_param_sens_board_z_off.get()))); + + _board_rotation = board_rotation_offset * board_rotation; + } +} + +void +VehicleAngularVelocity::Run() +{ + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // update corrections first to set _selected_sensor + SensorCorrectionsUpdate(); + + if (_sensor_control_available) { + // using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control + sensor_gyro_control_s sensor_data; + + if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); + + ParametersUpdate(); + SensorBiasUpdate(); + + // get the sensor data and correct for thermal errors (apply offsets and scale) + Vector3f rates{(Vector3f{sensor_data.xyz} - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; + + // correct for in-run bias errors + rates -= _bias; + + vehicle_angular_velocity_s angular_velocity; + angular_velocity.timestamp_sample = sensor_data.timestamp_sample; + rates.copyTo(angular_velocity.xyz); + angular_velocity.timestamp = hrt_absolute_time(); + + _vehicle_angular_velocity_pub.publish(angular_velocity); + } + + } else { + // otherwise fallback to using sensor_gyro (legacy that will be removed) + sensor_gyro_s sensor_data; + + if (_sensor_sub[_selected_sensor].update(&sensor_data)) { + perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); + + ParametersUpdate(); + SensorBiasUpdate(); + + // get the sensor data and correct for thermal errors + const Vector3f val{sensor_data.x, sensor_data.y, sensor_data.z}; + + // apply offsets and scale + Vector3f rates{(val - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; + + // correct for in-run bias errors + rates -= _bias; + + vehicle_angular_velocity_s angular_velocity; + angular_velocity.timestamp_sample = sensor_data.timestamp; + rates.copyTo(angular_velocity.xyz); + angular_velocity.timestamp = hrt_absolute_time(); + + _vehicle_angular_velocity_pub.publish(angular_velocity); + } + } + + perf_end(_cycle_perf); +} + +void +VehicleAngularVelocity::PrintStatus() +{ + PX4_INFO("selected sensor: %d", _selected_sensor); + + if (_selected_sensor_device_id != 0) { + PX4_INFO("using sensor_gyro_control: %d (%d)", _selected_sensor_device_id, _selected_sensor_control); + + } else { + PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor); + } + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_sensor_latency_perf); +} diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp new file mode 100644 index 000000000000..81ee9fff842f --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem +{ +public: + + VehicleAngularVelocity(); + virtual ~VehicleAngularVelocity(); + + void Run() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + + void ParametersUpdate(bool force = false); + void SensorBiasUpdate(bool force = false); + bool SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_board_rot, + + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off + ) + + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ + + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { /**< sensor data subscription */ + {this, ORB_ID(sensor_gyro), 0}, + {this, ORB_ID(sensor_gyro), 1}, + {this, ORB_ID(sensor_gyro), 2} + }; + + uORB::SubscriptionCallbackWorkItem _sensor_control_sub[MAX_SENSOR_COUNT] { /**< sensor control data subscription */ + {this, ORB_ID(sensor_gyro_control), 0}, + {this, ORB_ID(sensor_gyro_control), 1}, + {this, ORB_ID(sensor_gyro_control), 2} + }; + + matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + + matrix::Vector3f _offset; + matrix::Vector3f _scale; + matrix::Vector3f _bias; + + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; + perf_counter_t _sensor_latency_perf; + + uint32_t _selected_sensor_device_id{0}; + uint8_t _selected_sensor{0}; + uint8_t _selected_sensor_control{0}; + bool _sensor_control_available{false}; + +}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 026f73d8083b..db938ef244aa 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -50,6 +50,7 @@ using namespace sensors; using namespace DriverFramework; +using namespace matrix; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) : _parameters(parameters), _hil_enabled(hil_enabled) @@ -60,13 +61,12 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); memset(&_accel_diff, 0, sizeof(_accel_diff)); memset(&_gyro_diff, 0, sizeof(_gyro_diff)); - memset(&_mag_diff, 0, sizeof(_mag_diff)); + memset(&_mag_angle_diff, 0, sizeof(_mag_angle_diff)); // initialise the publication variables memset(&_info, 0, sizeof(_info)); for (unsigned i = 0; i < 3; i++) { - // Set scales to 1, offsets are zero initialized _corrections.gyro_scale_0[i] = 1.0f; _corrections.accel_scale_0[i] = 1.0f; _corrections.gyro_scale_1[i] = 1.0f; @@ -94,19 +94,19 @@ int VotedSensorsUpdate::init(sensor_combined_s &raw) raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; raw.timestamp = 0; - initialize_sensors(); + initializeSensors(); _selection_changed = true; return 0; } -void VotedSensorsUpdate::initialize_sensors() +void VotedSensorsUpdate::initializeSensors() { - init_sensor_class(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX); - init_sensor_class(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX); - init_sensor_class(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX); - init_sensor_class(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX); + initSensorClass(ORB_ID(sensor_gyro), _gyro, GYRO_COUNT_MAX); + initSensorClass(ORB_ID(sensor_mag), _mag, MAG_COUNT_MAX); + initSensorClass(ORB_ID(sensor_accel), _accel, ACCEL_COUNT_MAX); + initSensorClass(ORB_ID(sensor_baro), _baro, BARO_COUNT_MAX); } void VotedSensorsUpdate::deinit() @@ -128,13 +128,13 @@ void VotedSensorsUpdate::deinit() } } -void VotedSensorsUpdate::parameters_update() +void VotedSensorsUpdate::parametersUpdate() { /* fine tune board offset */ - matrix::Dcmf board_rotation_offset = matrix::Eulerf( - M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); + Dcmf board_rotation_offset = Eulerf( + M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation); @@ -210,7 +210,7 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ - config_ok = apply_gyro_calibration(h, &gscale, device_id); + config_ok = applyGyroCalibration(h, &gscale, device_id); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); @@ -298,7 +298,7 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ - config_ok = apply_accel_calibration(h, &ascale, device_id); + config_ok = applyAccelCalibration(h, &ascale, device_id); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); @@ -443,7 +443,7 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ - config_ok = apply_mag_calibration(h, &mscale, device_id); + config_ok = applyMagCalibration(h, &mscale, device_id); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); @@ -457,7 +457,7 @@ void VotedSensorsUpdate::parameters_update() } -void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::accelPoll(struct sensor_combined_s &raw) { float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 }; float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 }; @@ -488,7 +488,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) _accel_device_id[uorb_index] = accel_report.device_id; - matrix::Vector3f accel_data; + Vector3f accel_data; if (accel_report.integral_dt != 0) { /* @@ -500,9 +500,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) // convert the delta velocities to an equivalent acceleration before application of corrections float dt_inv = 1.e6f / accel_report.integral_dt; - accel_data = matrix::Vector3f(accel_report.x_integral * dt_inv, - accel_report.y_integral * dt_inv, - accel_report.z_integral * dt_inv); + accel_data = Vector3f(accel_report.x_integral * dt_inv, + accel_report.y_integral * dt_inv, + accel_report.z_integral * dt_inv); _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt; @@ -511,7 +511,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) // Correct each sensor for temperature effects // Filtering and/or downsampling of temperature should be performed in the driver layer - accel_data = matrix::Vector3f(accel_report.x, accel_report.y, accel_report.z); + accel_data = Vector3f(accel_report.x, accel_report.y, accel_report.z); // handle the case where this is our first output if (_last_accel_timestamp[uorb_index] == 0) { @@ -562,7 +562,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) } } -void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) +void VotedSensorsUpdate::gyroPoll(struct sensor_combined_s &raw) { float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 }; float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 }; @@ -593,7 +593,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) _gyro_device_id[uorb_index] = gyro_report.device_id; - matrix::Vector3f gyro_data; + Vector3f gyro_data; if (gyro_report.integral_dt != 0) { /* @@ -605,9 +605,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) // convert the delta angles to an equivalent angular rate before application of corrections float dt_inv = 1.e6f / gyro_report.integral_dt; - gyro_data = matrix::Vector3f(gyro_report.x_integral * dt_inv, - gyro_report.y_integral * dt_inv, - gyro_report.z_integral * dt_inv); + gyro_data = Vector3f(gyro_report.x_integral * dt_inv, + gyro_report.y_integral * dt_inv, + gyro_report.z_integral * dt_inv); _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt; @@ -668,7 +668,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) } } -void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) +void VotedSensorsUpdate::magPoll(vehicle_magnetometer_s &magnetometer) { for (int uorb_index = 0; uorb_index < _mag.subscription_count; uorb_index++) { bool mag_updated; @@ -694,10 +694,10 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) _mag.priority[uorb_index] = (uint8_t)priority; /* force a scale and offset update the first time we get data */ - parameters_update(); + parametersUpdate(); } - matrix::Vector3f vect(mag_report.x, mag_report.y, mag_report.z); + Vector3f vect(mag_report.x, mag_report.y, mag_report.z); vect = _mag_rotation[uorb_index] * vect; _last_magnetometer[uorb_index].timestamp = mag_report.timestamp; @@ -723,7 +723,7 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) } } -void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) +void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) { bool got_update = false; float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 }; @@ -758,7 +758,7 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) _baro_device_id[uorb_index] = baro_report.device_id; got_update = true; - matrix::Vector3f vect(baro_report.pressure, baro_report.temperature, 0.f); + Vector3f vect(baro_report.pressure, baro_report.temperature, 0.f); _last_airdata[uorb_index].timestamp = baro_report.timestamp; _last_airdata[uorb_index].baro_temp_celcius = baro_report.temperature; @@ -818,7 +818,7 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) } } -bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type) +bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type) { if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { @@ -875,12 +875,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n _info.enabled = true; _info.ok = false; - if (_info_pub == nullptr) { - _info_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(subsystem_info), _info_pub, &_info); - } + _info_pub.publish(_info); } } } @@ -892,7 +887,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n return false; } -void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, +void VotedSensorsUpdate::initSensorClass(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max) { int max_sensor_index = -1; @@ -922,7 +917,7 @@ void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, Sens } } -void VotedSensorsUpdate::print_status() +void VotedSensorsUpdate::printStatus() { PX4_INFO("gyro status:"); _gyro.voter.print(); @@ -935,7 +930,7 @@ void VotedSensorsUpdate::print_status() } bool -VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id) +VotedSensorsUpdate::applyGyroCalibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id) { #if defined(__PX4_NUTTX) @@ -949,7 +944,7 @@ VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calib } bool -VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id) +VotedSensorsUpdate::applyAccelCalibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id) { #if defined(__PX4_NUTTX) @@ -963,7 +958,7 @@ VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_cal } bool -VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id) +VotedSensorsUpdate::applyMagCalibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id) { #if defined(__PX4_NUTTX) @@ -980,41 +975,35 @@ VotedSensorsUpdate::apply_mag_calibration(DevHandle &h, const struct mag_calibra #endif } -void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata, - vehicle_magnetometer_s &magnetometer) +void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, + vehicle_magnetometer_s &magnetometer) { - _corrections_sub.update(&_corrections); - accel_poll(raw); - gyro_poll(raw); - mag_poll(magnetometer); - baro_poll(airdata); + accelPoll(raw); + gyroPoll(raw); + magPoll(magnetometer); + baroPoll(airdata); // publish sensor selection if changed if (_selection_changed) { _selection.timestamp = hrt_absolute_time(); - if (_sensor_selection_pub == nullptr) { - _sensor_selection_pub = orb_advertise(ORB_ID(sensor_selection), &_selection); - - } else { - orb_publish(ORB_ID(sensor_selection), _sensor_selection_pub, &_selection); - } + _sensor_selection_pub.publish(_selection); _selection_changed = false; } } -void VotedSensorsUpdate::check_failover() +void VotedSensorsUpdate::checkFailover() { - check_failover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); - check_failover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); - check_failover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); - check_failover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); + checkFailover(_accel, "Accel", subsystem_info_s::SUBSYSTEM_TYPE_ACC); + checkFailover(_gyro, "Gyro", subsystem_info_s::SUBSYSTEM_TYPE_GYRO); + checkFailover(_mag, "Mag", subsystem_info_s::SUBSYSTEM_TYPE_MAG); + checkFailover(_baro, "Baro", subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); } -void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) +void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) { if (_last_accel_timestamp[_accel.last_best_vote]) { raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - @@ -1023,7 +1012,7 @@ void VotedSensorsUpdate::set_relative_timestamps(sensor_combined_s &raw) } void -VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt) +VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt) { float accel_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared unsigned check_index = 0; // the number of sensors the primary has been checked against @@ -1072,7 +1061,7 @@ VotedSensorsUpdate::calc_accel_inconsistency(sensor_preflight_s &preflt) } } -void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) +void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt) { float gyro_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared unsigned check_index = 0; // the number of sensors the primary has been checked against @@ -1121,34 +1110,25 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt) } } -void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) +void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt) { - float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared + Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector + float mag_angle_diff_max = 0.0f; // the maximum angle difference unsigned check_index = 0; // the number of sensors the primary has been checked against // Check each sensor against the primary - for (int sensor_index = 0; sensor_index < _mag.subscription_count; sensor_index++) { - + for (int i = 0; i < _mag.subscription_count; i++) { // check that the sensor we are checking against is not the same as the primary - if ((_mag.priority[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) { - - float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary + if ((_mag.priority[i] > 0) && (i != _mag.last_best_vote)) { + // calculate angle to 3D magnetic field vector of the primary sensor + Vector3f current_mag(_last_magnetometer[i].magnetometer_ga); + float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle(); - // calculate mag_diff_sum_sq for the specified sensor against the primary - for (unsigned axis_index = 0; axis_index < 3; axis_index++) { - _mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f * - (_last_magnetometer[_mag.last_best_vote].magnetometer_ga[axis_index] - - _last_magnetometer[sensor_index].magnetometer_ga[axis_index]); - - mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index]; + // complementary filter to not fail/pass on single outliers + _mag_angle_diff[check_index] *= 0.95f; + _mag_angle_diff[check_index] += 0.05f * angle_error; - } - - // capture the largest sum value - if (mag_diff_sum_sq > mag_diff_sum_max_sq) { - mag_diff_sum_max_sq = mag_diff_sum_sq; - - } + mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); // increment the check index check_index++; @@ -1157,16 +1137,10 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // check to see if the maximum number of checks has been reached and break if (check_index >= 2) { break; - } } - // skip check if less than 2 sensors - if (check_index < 1) { - preflt.mag_inconsistency_ga = 0.0f; - - } else { - // get the vector length of the largest difference and write to the combined sensor struct - preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq); - } + // get the vector length of the largest difference and write to the combined sensor struct + // will be zero if there is only one magnetometer and hence nothing to compare + preflt.mag_inconsistency_angle = mag_angle_diff_max; } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 30629a1796a5..2e5833cc97a4 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -54,6 +54,8 @@ #include #include +#include +#include #include #include #include @@ -92,56 +94,57 @@ class VotedSensorsUpdate /** * This tries to find new sensor instances. This is called from init(), then it can be called periodically. */ - void initialize_sensors(); + void initializeSensors(); /** * deinitialize the object (we cannot use the destructor because it is called on the wrong thread) */ void deinit(); - void print_status(); + void printStatus(); /** - * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least + * call this whenever parameters got updated. Make sure to have initializeSensors() called at least * once before calling this. */ - void parameters_update(); + void parametersUpdate(); /** * read new sensor data */ - void sensors_poll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); + void sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer); /** - * set the relative timestamps of each sensor timestamp, based on the last sensors_poll, + * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, * so that the data can be published. */ - void set_relative_timestamps(sensor_combined_s &raw); + void setRelativeTimestamps(sensor_combined_s &raw); /** * check if a failover event occured. if so, report it. */ - void check_failover(); + void checkFailover(); - int num_gyros() const { return _gyro.subscription_count; } - int gyro_fd(int idx) const { return _gyro.subscription[idx]; } + int numGyros() const { return _gyro.subscription_count; } - int best_gyro_fd() const { return _gyro.subscription[_gyro.last_best_vote]; } + int gyroFd(int idx) const { return _gyro.subscription[idx]; } + + int bestGyroFd() const { return _gyro.subscription[_gyro.last_best_vote]; } /** * Calculates the magnitude in m/s/s of the largest difference between the primary and any other accel sensor */ - void calc_accel_inconsistency(sensor_preflight_s &preflt); + void calcAccelInconsistency(sensor_preflight_s &preflt); /** * Calculates the magnitude in rad/s of the largest difference between the primary and any other gyro sensor */ - void calc_gyro_inconsistency(sensor_preflight_s &preflt); + void calcGyroInconsistency(sensor_preflight_s &preflt); /** * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers */ - void calc_mag_inconsistency(sensor_preflight_s &preflt); + void calcMagInconsistency(sensor_preflight_s &preflt); private: @@ -169,45 +172,45 @@ class VotedSensorsUpdate unsigned int last_failover_count; }; - void init_sensor_class(const struct orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); + void initSensorClass(const orb_metadata *meta, SensorData &sensor_data, uint8_t sensor_count_max); /** * Poll the accelerometer for updated data. * - * @param raw Combined sensor data structure into which - * data should be returned. + * @param raw Combined sensor data structure into which + * data should be returned. */ - void accel_poll(struct sensor_combined_s &raw); + void accelPoll(sensor_combined_s &raw); /** * Poll the gyro for updated data. * - * @param raw Combined sensor data structure into which - * data should be returned. + * @param raw Combined sensor data structure into which + * data should be returned. */ - void gyro_poll(struct sensor_combined_s &raw); + void gyroPoll(sensor_combined_s &raw); /** * Poll the magnetometer for updated data. * - * @param raw Combined sensor data structure into which - * data should be returned. + * @param raw Combined sensor data structure into which + * data should be returned. */ - void mag_poll(vehicle_magnetometer_s &magnetometer); + void magPoll(vehicle_magnetometer_s &magnetometer); /** * Poll the barometer for updated data. * - * @param raw Combined sensor data structure into which - * data should be returned. + * @param raw Combined sensor data structure into which + * data should be returned. */ - void baro_poll(vehicle_air_data_s &airdata); + void baroPoll(vehicle_air_data_s &airdata); /** * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) */ - bool check_failover(SensorData &sensor, const char *sensor_name, const uint64_t type); + bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type); /** * Apply a gyro calibration. @@ -217,7 +220,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_gyro_calibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); + bool applyGyroCalibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); /** * Apply a accel calibration. @@ -227,8 +230,8 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_accel_calibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, - const int device_id); + bool applyAccelCalibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, + const int device_id); /** * Apply a mag calibration. @@ -238,51 +241,50 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_mag_calibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); + bool applyMagCalibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); + + SensorData _accel {}; + SensorData _gyro {}; + SensorData _mag {}; + SensorData _baro {}; - SensorData _gyro; - SensorData _accel; - SensorData _mag; - SensorData _baro; + orb_advert_t _mavlink_log_pub{nullptr}; - orb_advert_t _mavlink_log_pub = nullptr; + uORB::Publication _sensor_correction_pub{ORB_ID(sensor_correction)}; /**< handle to the sensor correction uORB topic */ + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ - sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ - vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ - vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX]; /**< latest sensor data from all sensors instances */ + sensor_combined_s _last_sensor_data[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ + vehicle_air_data_s _last_airdata[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ + vehicle_magnetometer_s _last_magnetometer[SENSOR_COUNT_MAX] {}; /**< latest sensor data from all sensors instances */ - uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */ + uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX] {}; /**< latest full timestamp */ - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - matrix::Dcmf _mag_rotation[MAG_COUNT_MAX]; /**< rotation matrix for the orientation that the external mag0 is mounted */ + matrix::Dcmf _board_rotation {}; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Dcmf _mag_rotation[MAG_COUNT_MAX] {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ const Parameters &_parameters; - const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */ + const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */ - float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */ - float _mag_diff[3][2]; /**< filtered mag differences between sensor instances (Ga) */ + float _accel_diff[3][2] {}; /**< filtered accel differences between IMU units (m/s/s) */ + float _gyro_diff[3][2] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ + float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ /* sensor thermal compensation */ uORB::Subscription _corrections_sub{ORB_ID(sensor_correction)}; - sensor_correction_s _corrections{}; /**< struct containing the sensor corrections to be subscribed from the uORB*/ + sensor_correction_s _corrections {}; /**< struct containing the sensor corrections to be published to the uORB */ /* sensor selection publication */ - struct sensor_selection_s _selection = {}; /**< struct containing the sensor selection to be published to the uORB*/ - orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */ - bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */ + sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ + bool _selection_changed{false}; /**< true when a sensor selection has changed and not been published */ - /* subsystem info publication */ - struct subsystem_info_s _info; - orb_advert_t _info_pub = nullptr; + subsystem_info_s _info {}; /**< subsystem info publication */ + uORB::PublicationQueued _info_pub{ORB_ID(subsystem_info)}; /* subsystem info publication */ - uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */ - uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {}; - uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {}; - uint32_t _mag_device_id[SENSOR_COUNT_MAX] = {}; + uint32_t _accel_device_id[SENSOR_COUNT_MAX] {}; /**< accel driver device id for each uorb instance */ + uint32_t _baro_device_id[SENSOR_COUNT_MAX] {}; /**< baro driver device id for each uorb instance */ + uint32_t _gyro_device_id[SENSOR_COUNT_MAX] {}; /**< gyro driver device id for each uorb instance */ + uint32_t _mag_device_id[SENSOR_COUNT_MAX] {}; /**< mag driver device id for each uorb instance */ }; - - } /* namespace sensors */ diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt index 23ebf62b9151..53b3d9f333c4 100644 --- a/src/modules/sih/CMakeLists.txt +++ b/src/modules/sih/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE modules__sih MAIN sih - STACK_MAIN 1200 STACK_MAX 1024 COMPILE_FLAGS SRCS diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index d7e4502b23f8..ca938242128e 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -408,19 +408,19 @@ void Sih::send_gps() void Sih::publish_sih() { - _gpos_gt.timestamp = hrt_absolute_time(); - _gpos_gt.lat = _gps_lat_noiseless; - _gpos_gt.lon = _gps_lon_noiseless; - _gpos_gt.alt = _gps_alt_noiseless; - _gpos_gt.vel_n = _v_I(0); - _gpos_gt.vel_e = _v_I(1); - _gpos_gt.vel_d = _v_I(2); + // publish angular velocity groundtruth + _vehicle_angular_velocity_gt.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed; + _vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed; + _vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed; - if (_gpos_gt_sub != nullptr) { - orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt); + if (_vehicle_angular_velocity_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_angular_velocity_groundtruth), _vehicle_angular_velocity_gt_pub, + &_vehicle_angular_velocity_gt); } else { - _gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt); + _vehicle_angular_velocity_gt_pub = orb_advertise(ORB_ID(vehicle_angular_velocity_groundtruth), + &_vehicle_angular_velocity_gt); } // publish attitude groundtruth @@ -429,15 +429,27 @@ void Sih::publish_sih() _att_gt.q[1] = _q(1); _att_gt.q[2] = _q(2); _att_gt.q[3] = _q(3); - _att_gt.rollspeed = _w_B(0); - _att_gt.pitchspeed = _w_B(1); - _att_gt.yawspeed = _w_B(2); - if (_att_gt_sub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt); + if (_att_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_pub, &_att_gt); + + } else { + _att_gt_pub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); + } + + _gpos_gt.timestamp = hrt_absolute_time(); + _gpos_gt.lat = _gps_lat_noiseless; + _gpos_gt.lon = _gps_lon_noiseless; + _gpos_gt.alt = _gps_alt_noiseless; + _gpos_gt.vel_n = _v_I(0); + _gpos_gt.vel_e = _v_I(1); + _gpos_gt.vel_d = _v_I(2); + + if (_gpos_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_pub, &_gpos_gt); } else { - _att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); + _gpos_gt_pub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt); } } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index a185668bb2d1..64553e658415 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -47,8 +47,9 @@ #include #include #include -#include // to publish groundtruth +#include // to publish groundtruth #include // to publish groundtruth +#include // to publish groundtruth #include extern "C" __EXPORT int sih_main(int argc, char *argv[]); @@ -103,14 +104,20 @@ class Sih : public ModuleBase, public ModuleParams PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION // to publish the gps position - struct vehicle_gps_position_s _vehicle_gps_pos {}; - orb_advert_t _vehicle_gps_pos_pub{nullptr}; + vehicle_gps_position_s _vehicle_gps_pos{}; + orb_advert_t _vehicle_gps_pos_pub{nullptr}; + + // angular velocity groundtruth + vehicle_angular_velocity_s _vehicle_angular_velocity_gt{}; + orb_advert_t _vehicle_angular_velocity_gt_pub{nullptr}; + // attitude groundtruth - struct vehicle_global_position_s _gpos_gt {}; - orb_advert_t _gpos_gt_sub{nullptr}; + vehicle_attitude_s _att_gt{}; + orb_advert_t _att_gt_pub{nullptr}; + // global position groundtruth - struct vehicle_attitude_s _att_gt {}; - orb_advert_t _att_gt_sub{nullptr}; + vehicle_global_position_s _gpos_gt{}; + orb_advert_t _gpos_gt_pub{nullptr}; int _parameter_update_sub {-1}; int _actuator_out_sub {-1}; diff --git a/src/modules/simulator/ledsim/CMakeLists.txt b/src/modules/simulator/ledsim/CMakeLists.txt index c8f9cbc012ed..b8d3ce64e019 100644 --- a/src/modules/simulator/ledsim/CMakeLists.txt +++ b/src/modules/simulator/ledsim/CMakeLists.txt @@ -32,4 +32,4 @@ ############################################################################ px4_add_library(drivers__ledsim led.cpp) -target_link_libraries(drivers__ledsim PRIVATE drivers__led drivers_board) +target_link_libraries(drivers__ledsim PRIVATE drivers__led) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 79519cb65473..2518e3ba45be 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -273,6 +274,7 @@ class Simulator : public ModuleParams static void *sending_trampoline(void *); // uORB publisher handlers + orb_advert_t _vehicle_angular_velocity_pub{nullptr}; orb_advert_t _attitude_pub{nullptr}; orb_advert_t _gpos_pub{nullptr}; orb_advert_t _lpos_pub{nullptr}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a829512d0063..89aeddc9edf1 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -408,25 +408,36 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg uint64_t timestamp = hrt_absolute_time(); + /* angular velocity */ + vehicle_angular_velocity_s hil_angular_velocity{}; + { + hil_angular_velocity.timestamp = timestamp; + + hil_angular_velocity.xyz[0] = hil_state.rollspeed; + hil_angular_velocity.xyz[1] = hil_state.pitchspeed; + hil_angular_velocity.xyz[2] = hil_state.yawspeed; + + // always publish ground truth attitude message + int hilstate_multi; + orb_publish_auto(ORB_ID(vehicle_angular_velocity_groundtruth), &_vehicle_angular_velocity_pub, &hil_angular_velocity, + &hilstate_multi, ORB_PRIO_HIGH); + } + /* attitude */ - struct vehicle_attitude_s hil_attitude = {}; + vehicle_attitude_s hil_attitude{}; { hil_attitude.timestamp = timestamp; matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - // always publish ground truth attitude message int hilstate_multi; orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH); } /* global position */ - struct vehicle_global_position_s hil_gpos = {}; + vehicle_global_position_s hil_gpos{}; { hil_gpos.timestamp = timestamp; @@ -662,6 +673,7 @@ void Simulator::poll_for_MAVLink_messages() if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { PX4_ERR("bind for UDP port %i failed (%i)", _port, errno); + ::close(_fd); return; } diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.h b/src/modules/temperature_compensation/TemperatureCompensationModule.h index a6baf93be805..df957930848b 100644 --- a/src/modules/temperature_compensation/TemperatureCompensationModule.h +++ b/src/modules/temperature_compensation/TemperatureCompensationModule.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include "TemperatureCompensation.h" diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 4e459ef0febf..10b43a3961d1 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -39,6 +39,7 @@ * @author Beat Küng */ +#include #include #include #include @@ -91,7 +92,7 @@ class TemperatureCalibration private: void publish_led_control(led_control_s &led_control); - orb_advert_t _led_control_pub = nullptr; + uORB::PublicationQueued _led_control_pub{ORB_ID(led_control)}; bool _force_task_exit = false; int _control_task = -1; // task handle for task @@ -353,13 +354,7 @@ void TemperatureCalibration::publish_led_control(led_control_s &led_control) { led_control.timestamp = hrt_absolute_time(); - - if (_led_control_pub == nullptr) { - _led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(led_control), _led_control_pub, &led_control); - } + _led_control_pub.publish(led_control); } int run_temperature_calibration(bool accel, bool baro, bool gyro) diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 6b3f4f0f818c..365127e73e28 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -39,16 +39,31 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat px4_add_module( MODULE modules__uORB MAIN uorb - STACK_MAIN 2100 SRCS + ORBSet.hpp + Publication.hpp + PublicationMulti.hpp + PublicationQueued.hpp Subscription.cpp + Subscription.hpp + SubscriptionCallback.hpp + SubscriptionInterval.hpp SubscriptionPollable.cpp + SubscriptionPollable.hpp uORB.cpp + uORB.h + uORBCommon.hpp + uORBCommunicator.hpp uORBDeviceMaster.cpp + uORBDeviceMaster.hpp uORBDeviceNode.cpp + uORBDeviceNode.hpp uORBMain.cpp uORBManager.cpp + uORBManager.hpp + uORBTopics.h uORBUtils.cpp + uORBUtils.hpp DEPENDS cdev uorb_msgs diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index c146c5e8531c..eac2a8f52d00 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * 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 @@ -38,15 +38,15 @@ #pragma once -#include +#include #include +#include namespace uORB { /** - * Base publication wrapper class, used in list traversal - * of various publications. + * Base publication wrapper class */ template class Publication @@ -56,13 +56,9 @@ class Publication /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub/sub, 0-based, -1 means - * don't publish as multi + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - Publication(const orb_metadata *meta, int priority = -1) : _meta(meta), _priority(priority) {} - + Publication(const orb_metadata *meta) : _meta(meta) {} ~Publication() { orb_unadvertise(_handle); } /** @@ -71,48 +67,29 @@ class Publication */ bool publish(const T &data) { - bool updated = false; - if (_handle != nullptr) { - if (orb_publish(_meta, _handle, &data) != PX4_OK) { - PX4_ERR("%s publish fail", _meta->o_name); - - } else { - updated = true; - } + return (orb_publish(_meta, _handle, &data) == PX4_OK); } else { - orb_advert_t handle = nullptr; - - if (_priority > 0) { - int instance; - handle = orb_advertise_multi(_meta, &data, &instance, _priority); - - } else { - handle = orb_advertise(_meta, &data); - } + orb_advert_t handle = orb_advertise(_meta, &data); if (handle != nullptr) { _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advert fail", _meta->o_name); + return true; } } - return updated; + return false; } protected: const orb_metadata *_meta; - const int _priority; orb_advert_t _handle{nullptr}; }; /** - * The publication base class as a list node. + * The publication class with data embedded. */ template class PublicationData : public Publication @@ -121,11 +98,9 @@ class PublicationData : public Publication /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub, 0-based. + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - PublicationData(const orb_metadata *meta, int priority = -1) : Publication(meta, priority) {} + PublicationData(const orb_metadata *meta) : Publication(meta) {} ~PublicationData() = default; T &get() { return _data; } diff --git a/src/modules/uORB/PublicationMulti.hpp b/src/modules/uORB/PublicationMulti.hpp new file mode 100644 index 000000000000..f00acfa753ca --- /dev/null +++ b/src/modules/uORB/PublicationMulti.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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 Publication.hpp + * + */ + +#pragma once + +#include +#include +#include + +namespace uORB +{ + +/** + * Base publication multi wrapper class + */ +template +class PublicationMulti +{ +public: + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0 means don't publish as multi + */ + PublicationMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + _meta(meta), + _priority(priority) + {} + + ~PublicationMulti() { orb_unadvertise(_handle); } + + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + if (_handle != nullptr) { + return (orb_publish(_meta, _handle, &data) == PX4_OK); + + } else { + int instance = 0; + orb_advert_t handle = orb_advertise_multi(_meta, &data, &instance, _priority); + + if (handle != nullptr) { + _handle = handle; + return true; + } + } + + return false; + } + +protected: + const orb_metadata *_meta; + + orb_advert_t _handle{nullptr}; + + const uint8_t _priority; +}; + +/** + * The publication multi class with data embedded. + */ +template +class PublicationMultiData : public PublicationMulti +{ +public: + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub + */ + PublicationMultiData(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + PublicationMulti(meta, priority) + {} + + ~PublicationMultiData() = default; + + T &get() { return _data; } + void set(const T &data) { _data = data; } + + // Publishes the embedded struct. + bool update() { return PublicationMulti::publish(_data); } + bool update(const T &data) + { + _data = data; + return PublicationMulti::publish(_data); + } + +private: + T _data{}; +}; + +} // namespace uORB diff --git a/src/drivers/imu/icm20948/gyro.h b/src/modules/uORB/PublicationQueued.hpp similarity index 63% rename from src/drivers/imu/icm20948/gyro.h rename to src/modules/uORB/PublicationQueued.hpp index cff6c8c649b3..79ee1550de81 100644 --- a/src/drivers/imu/icm20948/gyro.h +++ b/src/modules/uORB/PublicationQueued.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,36 +31,64 @@ * ****************************************************************************/ +/** + * @file PublicationQueued.hpp + * + */ + #pragma once -#include -#include +#include +#include #include -class ICM20948; +namespace uORB +{ /** - * Helper class implementing the gyro driver node. + * Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) */ -class ICM20948_gyro : public device::CDev +template +class PublicationQueued { public: - ICM20948_gyro(ICM20948 *parent, const char *path); - ~ICM20948_gyro(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + */ + PublicationQueued(const orb_metadata *meta) : _meta(meta) {} + ~PublicationQueued() + { + //orb_unadvertise(_handle); + } - virtual int init(); + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + if (_handle != nullptr) { + return (orb_publish(_meta, _handle, &data) == PX4_OK); -protected: - friend class ICM20948; + } else { + orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH); + + if (handle != nullptr) { + _handle = handle; + return true; + } + } - void parent_poll_notify(); + return false; + } -private: - ICM20948 *_parent; +protected: + const orb_metadata *_meta; - orb_advert_t _gyro_topic{nullptr}; - int _gyro_orb_class_instance{-1}; - int _gyro_class_instance{-1}; + orb_advert_t _handle{nullptr}; }; + +} // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index bc4987c895a1..9268a21ad9a3 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -42,36 +42,59 @@ namespace uORB { -bool Subscription::subscribe() +bool +Subscription::subscribe() { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - _node = device_master->getDeviceNode(_meta, _instance); + // valid ORB_ID required + if (_meta == nullptr) { + return false; + } + // check if already subscribed if (_node != nullptr) { - _node->add_internal_subscriber(); + return true; + } + + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - // If there were any previous publications, allow the subscriber to read them - const unsigned curr_gen = _node->published_message_count(); - const unsigned q_size = _node->get_queue_size(); + if (device_master != nullptr) { + uORB::DeviceNode *node = device_master->getDeviceNode(_meta, _instance); - _last_generation = curr_gen - (q_size < curr_gen ? q_size : curr_gen); + if (node != nullptr) { + _node = node; + _node->add_internal_subscriber(); - return true; + // If there were any previous publications, allow the subscriber to read them + const unsigned curr_gen = _node->published_message_count(); + const uint8_t q_size = _node->get_queue_size(); + + if (q_size < curr_gen) { + _last_generation = curr_gen - q_size; + + } else { + _last_generation = 0; + } + + return true; + } } return false; } -void Subscription::unsubscribe() +void +Subscription::unsubscribe() { if (_node != nullptr) { _node->remove_internal_subscriber(); } + _node = nullptr; _last_generation = 0; } -bool Subscription::init() +bool +Subscription::init() { if (_meta != nullptr) { // this throttles the relatively expensive calls to getDeviceNode() @@ -90,18 +113,8 @@ bool Subscription::init() return false; } -bool Subscription::forceInit() -{ - if (_node == nullptr) { - // reset generation to force subscription attempt - _last_generation = 0; - return subscribe(); - } - - return false; -} - -bool Subscription::update(uint64_t *time, void *dst) +bool +Subscription::update(uint64_t *time, void *dst) { if ((time != nullptr) && (dst != nullptr) && published()) { // always copy data to dst regardless of update diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 6c98d3871806..47f87112c98a 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -63,16 +63,34 @@ class Subscription */ Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance) { - init(); + subscribe(); } - ~Subscription() { unsubscribe(); } + ~Subscription() + { + unsubscribe(); + } - bool init(); - bool forceInit(); + bool subscribe(); + void unsubscribe(); bool valid() const { return _node != nullptr; } - bool published() { return valid() ? _node->is_published() : init(); } + bool published() + { + if (valid()) { + return _node->is_published(); + } + + // try to initialize + if (init()) { + // check again if valid + if (valid()) { + return _node->is_published(); + } + } + + return false; + } /** * Check if there is a new update. @@ -102,8 +120,6 @@ class Subscription */ bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; } - hrt_abstime last_update() { return published() ? _node->last_update() : 0; } - uint8_t get_instance() const { return _instance; } orb_id_t get_topic() const { return _meta; } @@ -111,10 +127,9 @@ class Subscription friend class SubscriptionCallback; - DeviceNode *get_node() { return _node; } + DeviceNode *get_node() { return _node; } - bool subscribe(); - void unsubscribe(); + bool init(); DeviceNode *_node{nullptr}; const orb_metadata *_meta{nullptr}; diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index 01722bc6819e..1324f6ba24e2 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -40,7 +40,7 @@ #include #include -#include +#include namespace uORB { @@ -78,7 +78,7 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNoderegister_callback(this)) { ret = true; } @@ -124,7 +124,7 @@ class SubscriptionCallbackWorkItem : public SubscriptionCallback void call() override { // schedule immediately if no interval, otherwise check time elapsed - if ((_interval == 0) || (hrt_elapsed_time(&_last_update) >= _interval)) { + if ((_interval_us == 0) || (hrt_elapsed_time_atomic(&_last_update) >= _interval_us)) { _work_item->ScheduleNow(); } } diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 253c0570d2ce..5c701eb98661 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -64,13 +64,15 @@ class SubscriptionInterval */ SubscriptionInterval(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) : _subscription{meta, instance}, - _interval(interval_us) + _interval_us(interval_us) {} SubscriptionInterval() : _subscription{nullptr} {} ~SubscriptionInterval() = default; + bool subscribe() { return _subscription.subscribe(); } + bool published() { return _subscription.published(); } /** @@ -78,7 +80,7 @@ class SubscriptionInterval * */ bool updated() { - if (published() && (hrt_elapsed_time(&_last_update) >= _interval)) { + if (published() && (hrt_elapsed_time(&_last_update) >= _interval_us)) { return _subscription.updated(); } @@ -118,15 +120,24 @@ class SubscriptionInterval uint8_t get_instance() const { return _subscription.get_instance(); } orb_id_t get_topic() const { return _subscription.get_topic(); } - uint32_t get_interval() const { return _interval; } - void set_interval(uint32_t interval) { _interval = interval; } + /** + * Set the interval in microseconds + * @param interval The interval in microseconds. + */ + void set_interval_us(uint32_t interval) { _interval_us = interval; } + + /** + * Set the interval in milliseconds + * @param interval The interval in milliseconds. + */ + void set_interval_ms(uint32_t interval) { _interval_us = interval * 1000; } protected: Subscription _subscription; uint64_t _last_update{0}; // last update in microseconds - uint32_t _interval{0}; // maximum update interval in microseconds + uint32_t _interval_us{0}; // maximum update interval in microseconds }; diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 6ccdc4d01627..84127955f4e8 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -102,10 +102,10 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in return -ENOMEM; } - /* construct the new node */ + /* construct the new node, passing the ownership of path to it */ uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority); - /* if we didn't get a device, that's bad */ + /* if we didn't get a device, that's bad, free the path too */ if (node == nullptr) { free((void *)devpath); return -ENOMEM; @@ -133,9 +133,6 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in } } - /* also discard the name now */ - free((void *)devpath); - } else { // add to the node map;. _node_list.add(node); @@ -430,6 +427,10 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) { + if (meta == nullptr) { + return nullptr; + } + lock(); uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); unlock(); diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index d6aad0439842..e152f891779f 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -33,7 +33,6 @@ #include "uORBDeviceNode.hpp" -#include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -70,6 +69,8 @@ uORB::DeviceNode::~DeviceNode() if (_data != nullptr) { delete[] _data; } + + CDev::unregister_driver_and_memory(); } int @@ -220,18 +221,6 @@ uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation) return update_time; } -hrt_abstime -uORB::DeviceNode::last_update() -{ - ATOMIC_ENTER; - - const hrt_abstime update_time = _last_update; - - ATOMIC_LEAVE; - - return update_time; -} - ssize_t uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) { diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index ef0a7b08db3b..168293a3566a 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -56,7 +56,7 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode public: DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority, uint8_t queue_size = 1); - ~DeviceNode(); + virtual ~DeviceNode(); // no copy, assignment, move, move assignment DeviceNode(const DeviceNode &) = delete; @@ -196,14 +196,6 @@ class uORB::DeviceNode : public cdev::CDev, public ListNode int get_priority() const { return _priority; } void set_priority(uint8_t priority) { _priority = priority; } - /** - * Copies the timestamp of the last update atomically. - * - * @return uint64_t - * Returns the timestamp of the most recent data. - */ - hrt_abstime last_update(); - /** * Copies data and the corresponding generation * from a node to the buffer provided. diff --git a/src/modules/uORB/uORBManager.cpp b/src/modules/uORB/uORBManager.cpp index 20c9bf843298..7febe89353f4 100644 --- a/src/modules/uORB/uORBManager.cpp +++ b/src/modules/uORB/uORBManager.cpp @@ -55,6 +55,17 @@ bool uORB::Manager::initialize() return _Instance != nullptr; } +bool uORB::Manager::terminate() +{ + if (_Instance != nullptr) { + delete _Instance; + _Instance = nullptr; + return true; + } + + return false; +} + uORB::Manager::Manager() { #ifdef ORB_USE_PUBLISHER_RULES diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 42d195b26fe2..1969e3000b9a 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -75,6 +75,12 @@ class uORB::Manager */ static bool initialize(); + /** + * Terminate the singleton. Call this after everything else. + * @return true on success + */ + static bool terminate(); + /** * Method to get the singleton instance for the uORB::Manager. * Make sure initialize() is called first. @@ -416,7 +422,7 @@ class uORB::Manager private: //class methods Manager(); - ~Manager(); + virtual ~Manager(); #ifdef ORB_COMMUNICATOR /** diff --git a/src/modules/uORB/uORB_tests/CMakeLists.txt b/src/modules/uORB/uORB_tests/CMakeLists.txt index ac8472a4a5e9..ea77e368f3ea 100644 --- a/src/modules/uORB/uORB_tests/CMakeLists.txt +++ b/src/modules/uORB/uORB_tests/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE modules__uORB__uORB_tests MAIN uorb_tests - STACK_MAIN 2048 PRIORITY "SCHED_PRIORITY_MAX" SRCS uORB_tests_main.cpp diff --git a/src/modules/vmount/CMakeLists.txt b/src/modules/vmount/CMakeLists.txt index 8b996ffeb291..069a1cca133c 100644 --- a/src/modules/vmount/CMakeLists.txt +++ b/src/modules/vmount/CMakeLists.txt @@ -34,7 +34,6 @@ px4_add_module( MODULE drivers__vmount MAIN vmount COMPILE_FLAGS - STACK_MAIN 1024 SRCS input.cpp input_mavlink.cpp @@ -48,4 +47,4 @@ px4_add_module( git_ecl ecl_geo ) - + diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 91b4f2dc7c3b..dbceb7d15f0c 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -39,7 +39,7 @@ */ #include "input_mavlink.h" -#include +#include #include #include #include @@ -333,7 +333,7 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) { - vehicle_command_ack_s vehicle_command_ack = {}; + vehicle_command_ack_s vehicle_command_ack{}; vehicle_command_ack.timestamp = hrt_absolute_time(); vehicle_command_ack.command = cmd->command; @@ -341,13 +341,8 @@ void InputMavlinkCmdMount::_ack_vehicle_command(vehicle_command_s *cmd) vehicle_command_ack.target_system = cmd->source_system; vehicle_command_ack.target_component = cmd->source_component; - if (_vehicle_command_ack_pub == nullptr) { - _vehicle_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &vehicle_command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _vehicle_command_ack_pub, &vehicle_command_ack); - } + uORB::PublicationQueued cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); } void InputMavlinkCmdMount::print_status() diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index 424e1a60ebd8..b3661466ec75 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -43,13 +43,11 @@ #include "input_rc.h" #include -#include #include +#include namespace vmount { - - /** ** class InputMavlinkROI ** Input based on the vehicle_roi topic @@ -95,7 +93,6 @@ class InputMavlinkCmdMount : public InputBase void _ack_vehicle_command(vehicle_command_s *cmd); int _vehicle_command_sub = -1; - orb_advert_t _vehicle_command_ack_pub = nullptr; bool _stabilize[3] = { false, false, false }; int32_t _mav_sys_id{1}; ///< our mavlink system id diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 4e10867de8ed..1d508be2b10f 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -54,16 +54,9 @@ OutputMavlink::OutputMavlink(const OutputConfig &output_config) { } -OutputMavlink::~OutputMavlink() -{ - if (_vehicle_command_pub) { - orb_unadvertise(_vehicle_command_pub); - } -} - int OutputMavlink::update(const ControlData *control_data) { - vehicle_command_s vehicle_command = {}; + vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id; vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id; @@ -82,18 +75,7 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param1 = vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING; } - if (_vehicle_command_pub) { - orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); - - } else { - _vehicle_command_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vehicle_command, - vehicle_command_s::ORB_QUEUE_LENGTH); - } - - } - - if (!_vehicle_command_pub) { - return 0; + _vehicle_command_pub.publish(vehicle_command); } _handle_position_update(); @@ -110,7 +92,7 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F; vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F; - orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); + _vehicle_command_pub.publish(vehicle_command); _last_update = t; diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 24c2272afa8f..b4a6ce3f0d96 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -41,13 +41,11 @@ #include "output.h" -#include - +#include +#include namespace vmount { - - /** ** class OutputMavlink * Output via vehicle_command topic @@ -56,14 +54,15 @@ class OutputMavlink : public OutputBase { public: OutputMavlink(const OutputConfig &output_config); - virtual ~OutputMavlink(); + virtual ~OutputMavlink() = default; virtual int update(const ControlData *control_data); virtual void print_status(); private: - orb_advert_t _vehicle_command_pub = nullptr; + + uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; }; diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index a5f5dabc8df4..e6316b14593f 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -33,15 +33,11 @@ px4_add_module( MODULE modules__vtol_att_control MAIN vtol_att_control - STACK_MAIN 1300 - COMPILE_FLAGS SRCS vtol_att_control_main.cpp tiltrotor.cpp vtol_type.cpp tailsitter.cpp standard.cpp - DEPENDS - pwm_limit ) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e535e78f507b..bc72d2832cb2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 - 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -49,18 +49,14 @@ #include "vtol_att_control_main.h" #include #include +#include using namespace matrix; -namespace VTOL_att_control -{ -VtolAttitudeControl *g_control; -} - -/** -* Constructor -*/ -VtolAttitudeControl::VtolAttitudeControl() +VtolAttitudeControl::VtolAttitudeControl() : + WorkItem(px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle")), + _loop_interval_perf(perf_alloc(PC_INTERVAL, "vtol_att_control: interval")) { _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -105,66 +101,30 @@ VtolAttitudeControl::VtolAttitudeControl() _vtol_type = new Standard(this); } else { - _task_should_exit = true; + exit_and_cleanup(); } } -/** -* Destructor -*/ VtolAttitudeControl::~VtolAttitudeControl() { - if (_control_task != -1) { - /* task wakes up every 100ms or so at the longest */ - _task_should_exit = true; - - /* wait for a second for the task to quit at our request */ - unsigned i = 0; - - do { - /* wait 20ms */ - px4_usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - px4_task_delete(_control_task); - break; - } - } while (_control_task != -1); - } - - // free memory used by instances of base class VtolType - if (_vtol_type != nullptr) { - delete _vtol_type; - } - - VTOL_att_control::g_control = nullptr; + perf_free(_loop_perf); + perf_free(_loop_interval_perf); } -/** -* Check for inputs from mc attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_mc_poll() +bool +VtolAttitudeControl::init() { - bool updated; - orb_check(_actuator_inputs_mc, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + if (!_actuator_inputs_mc.register_callback()) { + PX4_ERR("MC actuator controls callback registration failed!"); + return false; } -} -/** -* Check for inputs from fw attitude controller. -*/ -void VtolAttitudeControl::actuator_controls_fw_poll() -{ - bool updated; - orb_check(_actuator_inputs_fw, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + if (!_actuator_inputs_fw.register_callback()) { + PX4_ERR("FW actuator controls callback registration failed!"); + return false; } + + return true; } /** @@ -193,20 +153,15 @@ VtolAttitudeControl::handle_command() // This might not be optimal but is better than no response at all. if (_vehicle_cmd.from_external) { - vehicle_command_ack_s command_ack = {}; + vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = _vehicle_cmd.command; command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; command_ack.target_system = _vehicle_cmd.source_system; command_ack.target_component = _vehicle_cmd.source_component; - if (_v_cmd_ack_pub == nullptr) { - _v_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(vehicle_command_ack), _v_cmd_ack_pub, &command_ack); - } + uORB::PublicationQueued command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); } } } @@ -244,9 +199,6 @@ VtolAttitudeControl::is_fixed_wing_requested() return to_fw; } -/* - * Abort front transition - */ void VtolAttitudeControl::abort_front_transition(const char *reason) { @@ -257,9 +209,6 @@ VtolAttitudeControl::abort_front_transition(const char *reason) } } -/** -* Update parameters. -*/ int VtolAttitudeControl::parameters_update() { @@ -337,29 +286,53 @@ VtolAttitudeControl::parameters_update() return OK; } -int -VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +void +VtolAttitudeControl::Run() { - VTOL_att_control::g_control->task_main(); - return 0; -} + if (should_exit()) { + _actuator_inputs_fw.unregister_callback(); + _actuator_inputs_mc.unregister_callback(); + exit_and_cleanup(); + return; + } -void VtolAttitudeControl::task_main() -{ - /* do subscriptions */ - _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); - _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + if (!_initialized) { + parameters_update(); // initialize parameter cache + + if (_vtol_type->init()) { + _initialized = true; + + } else { + exit_and_cleanup(); + return; + } + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + const bool updated_fw_in = _actuator_inputs_fw.update(&_actuators_fw_in); + const bool updated_mc_in = _actuator_inputs_mc.update(&_actuators_mc_in); - parameters_update(); // initialize parameter cache + // run on actuator publications corresponding to VTOL mode + bool should_run = false; - _task_should_exit = !_vtol_type->init(); + switch (_vtol_type->get_mode()) { + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + should_run = updated_fw_in || updated_mc_in; + break; - /* wakeup source*/ - px4_pollfd_struct_t fds[1] = {}; - fds[0].fd = _actuator_inputs_mc; - fds[0].events = POLLIN; + case mode::ROTARY_WING: + should_run = updated_mc_in; + break; - while (!_task_should_exit) { + case mode::FIXED_WING: + should_run = updated_fw_in; + break; + } + + if (should_run) { /* only update parameters if they changed */ if (_params_sub.updated()) { /* read from param to clear updated flag */ @@ -370,35 +343,6 @@ void VtolAttitudeControl::task_main() parameters_update(); } - // run vtol_att on MC actuator publications, unless in full FW mode - switch (_vtol_type->get_mode()) { - case mode::TRANSITION_TO_FW: - case mode::TRANSITION_TO_MC: - case mode::ROTARY_WING: - fds[0].fd = _actuator_inputs_mc; - break; - - case mode::FIXED_WING: - fds[0].fd = _actuator_inputs_fw; - break; - } - - /* wait for up to 100ms for data */ - int pret = px4_poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_ERR("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - px4_usleep(100000); - continue; - } - _v_control_mode_sub.update(&_v_control_mode); _manual_control_sp_sub.update(&_manual_control_sp); _v_att_sub.update(&_v_att); @@ -409,192 +353,172 @@ void VtolAttitudeControl::task_main() _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); vehicle_cmd_poll(); - actuator_controls_fw_poll(); - actuator_controls_mc_poll(); - // update the vtol state machine which decides which mode we are in - _vtol_type->update_vtol_state(); + // check if mc and fw sp were updated + bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); + bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); - // reset transition command if not auto control - if (_v_control_mode.flag_control_manual_enabled) { - if (_vtol_type->get_mode() == mode::ROTARY_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; - - } else if (_vtol_type->get_mode() == mode::FIXED_WING) { - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; - - } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { - /* We want to make sure that a mode change (manual>auto) during the back transition - * doesn't result in an unsafe state. This prevents the instant fall back to - * fixed-wing on the switch from manual to auto */ - _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + // update the vtol state machine which decides which mode we are in + if (mc_att_sp_updated || fw_att_sp_updated) { + _vtol_type->update_vtol_state(); + + // reset transition command if not auto control + if (_v_control_mode.flag_control_manual_enabled) { + if (_vtol_type->get_mode() == mode::ROTARY_WING) { + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + + } else if (_vtol_type->get_mode() == mode::FIXED_WING) { + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; + + } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { + /* We want to make sure that a mode change (manual>auto) during the back transition + * doesn't result in an unsafe state. This prevents the instant fall back to + * fixed-wing on the switch from manual to auto */ + _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; + } } } + + // check in which mode we are in and call mode specific functions - if (_vtol_type->get_mode() == mode::ROTARY_WING) { + switch (_vtol_type->get_mode()) { + case mode::TRANSITION_TO_FW: + case mode::TRANSITION_TO_MC: + // vehicle is doing a transition + _vtol_vehicle_status.vtol_in_trans_mode = true; + _vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition + _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + + _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + + if (mc_att_sp_updated || fw_att_sp_updated) { + + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + + _vtol_type->update_transition_state(); + _v_att_sp_pub.publish(_v_att_sp); + } - _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); + break; + case mode::ROTARY_WING: // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; _vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.in_transition_to_fw = false; - // got data from mc attitude controller - _vtol_type->update_mc_state(); + if (mc_att_sp_updated) { + // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept + if (!_v_control_mode.flag_armed) { + Quatf().copyTo(_mc_virtual_att_sp.q_d); + Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); + Quatf().copyTo(_v_att_sp.q_d); + Vector3f().copyTo(_v_att_sp.thrust_body); + } + } - } else if (_vtol_type->get_mode() == mode::FIXED_WING) { + _vtol_type->update_mc_state(); + _v_att_sp_pub.publish(_v_att_sp); - _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); + break; + case mode::FIXED_WING: // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; _vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.in_transition_to_fw = false; - _vtol_type->update_fw_state(); - - } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC || _vtol_type->get_mode() == mode::TRANSITION_TO_FW) { - - _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); - _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); - - // vehicle is doing a transition - _vtol_vehicle_status.vtol_in_trans_mode = true; - _vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition - _vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW); + if (fw_att_sp_updated) { + _vtol_type->update_fw_state(); + _v_att_sp_pub.publish(_v_att_sp); + } - _vtol_type->update_transition_state(); + break; } _vtol_type->fill_actuator_outputs(); + _actuators_0_pub.publish(_actuators_out_0); + _actuators_1_pub.publish(_actuators_out_1); - // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept - if (!_v_control_mode.flag_armed) { - Quatf().copyTo(_mc_virtual_att_sp.q_d); - Vector3f().copyTo(_mc_virtual_att_sp.thrust_body); - Quatf().copyTo(_v_att_sp.q_d); - Vector3f().copyTo(_v_att_sp.thrust_body); - } - - /* Only publish if the proper mode(s) are enabled */ - if (_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) { - - if (_v_att_sp_pub != nullptr) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); - - } else { - /* advertise and publish */ - _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); - } + // Advertise/Publish vtol vehicle status + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub.publish(_vtol_vehicle_status); + } - if (_actuators_0_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + perf_end(_loop_perf); +} - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } +int +VtolAttitudeControl::task_spawn(int argc, char *argv[]) +{ + VtolAttitudeControl *instance = new VtolAttitudeControl(); - if (_actuators_1_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + if (instance->init()) { + return PX4_OK; } - /*Advertise/Publish vtol vehicle status*/ - _vtol_vehicle_status.timestamp = hrt_absolute_time(); - - if (_vtol_vehicle_status_pub != nullptr) { - orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); - - } else { - _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); - } + } else { + PX4_ERR("alloc failed"); } - PX4_WARN("exit"); - _control_task = -1; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } int -VtolAttitudeControl::start() +VtolAttitudeControl::custom_command(int argc, char *argv[]) { - /* start the task */ - _control_task = px4_task_spawn_cmd("vtol_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL + 1, - 1320, - (px4_main_t)&VtolAttitudeControl::task_main_trampoline, - nullptr); - - if (_control_task < 0) { - PX4_WARN("task start failed"); - return -errno; - } - - return OK; + return print_usage("unknown command"); } - -int vtol_att_control_main(int argc, char *argv[]) +int +VtolAttitudeControl::print_usage(const char *reason) { - if (argc < 2) { - PX4_WARN("usage: vtol_att_control {start|stop|status}"); - return 1; + if (reason) { + PX4_WARN("%s\n", reason); } - if (!strcmp(argv[1], "start")) { + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +fw_att_control is the fixed wing attitude controller. +)DESCR_STR"); - if (VTOL_att_control::g_control != nullptr) { - PX4_WARN("already running"); - return 0; - } + PRINT_MODULE_USAGE_COMMAND("start"); - VTOL_att_control::g_control = new VtolAttitudeControl; + PRINT_MODULE_USAGE_NAME("vtol_att_control", "controller"); - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("alloc failed"); - return 1; - } + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - if (OK != VTOL_att_control::g_control->start()) { - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - PX4_WARN("start failed"); - return 1; - } - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - if (VTOL_att_control::g_control == nullptr) { - PX4_WARN("not running"); - return 0; - } - - delete VTOL_att_control::g_control; - VTOL_att_control::g_control = nullptr; - return 0; - } + return 0; +} - if (!strcmp(argv[1], "status")) { - if (VTOL_att_control::g_control) { - PX4_WARN("running"); +int +VtolAttitudeControl::print_status() +{ + PX4_INFO("Running"); - } else { - PX4_WARN("not running"); - } + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); - return 0; - } + return PX4_OK; +} - PX4_WARN("unrecognized command"); - return 1; +int vtol_att_control_main(int argc, char *argv[]) +{ + return VtolAttitudeControl::main(argc, argv); } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 4a381b3ab65c..359fdccdabfb 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,26 +46,29 @@ * @author Andreas Antener * */ -#ifndef VTOL_ATT_CONTROL_MAIN_H -#define VTOL_ATT_CONTROL_MAIN_H -#include -#include -#include -#include +#pragma once #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 @@ -75,25 +78,37 @@ #include #include #include -#include #include -#include "tiltrotor.h" -#include "tailsitter.h" #include "standard.h" - +#include "tailsitter.h" +#include "tiltrotor.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl +class VtolAttitudeControl : public ModuleBase, public px4::WorkItem { public: VtolAttitudeControl(); ~VtolAttitudeControl(); - int start(); /* start the task and return OK on success */ + /** @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); + + void Run() override; + + bool init(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + bool is_fixed_wing_requested(); void abort_front_transition(const char *reason); @@ -116,15 +131,10 @@ class VtolAttitudeControl struct Params *get_params() {return &_params;} - private: -//******************flags & handlers****************************************************** - bool _task_should_exit{false}; - int _control_task{-1}; //task handle for VTOL attitude controller - /* handlers for subscriptions */ - int _actuator_inputs_fw{-1}; //topic on which the fw_att_controller publishes actuator inputs - int _actuator_inputs_mc{-1}; //topic on which the mc_att_controller publishes actuator inputs + uORB::SubscriptionCallbackWorkItem _actuator_inputs_fw{this, ORB_ID(actuator_controls_virtual_fw)}; + uORB::SubscriptionCallbackWorkItem _actuator_inputs_mc{this, ORB_ID(actuator_controls_virtual_mc)}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; // airspeed subscription uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; @@ -140,15 +150,12 @@ class VtolAttitudeControl uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; - //handlers for publishers - orb_advert_t _actuators_0_pub{nullptr}; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle - orb_advert_t _v_att_sp_pub{nullptr}; - orb_advert_t _v_cmd_ack_pub{nullptr}; - orb_advert_t _vtol_vehicle_status_pub{nullptr}; - orb_advert_t _actuators_1_pub{nullptr}; + uORB::Publication _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) + uORB::Publication _actuators_1_pub{ORB_ID(actuator_controls_1)}; + uORB::Publication _v_att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Publication _vtol_vehicle_status_pub{ORB_ID(vtol_vehicle_status)}; -//*******************data containers*********************************************************** + orb_advert_t _mavlink_log_pub{nullptr}; // mavlink log uORB handle vehicle_attitude_setpoint_s _v_att_sp{}; //vehicle attitude setpoint vehicle_attitude_setpoint_s _fw_virtual_att_sp{}; // virtual fw attitude setpoint @@ -167,7 +174,7 @@ class VtolAttitudeControl vehicle_command_s _vehicle_cmd{}; vehicle_control_mode_s _v_control_mode{}; //vehicle control mode vehicle_land_detected_s _land_detected{}; - vehicle_local_position_s _local_pos{}; + vehicle_local_position_s _local_pos{}; vehicle_local_position_setpoint_s _local_pos_sp{}; vtol_vehicle_status_s _vtol_vehicle_status{}; @@ -202,23 +209,19 @@ class VtolAttitudeControl /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ - int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; - bool _abort_front_transition{false}; + int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; + bool _abort_front_transition{false}; - VtolType *_vtol_type{nullptr}; // base class for different vtol types + VtolType *_vtol_type{nullptr}; // base class for different vtol types -//*****************Member functions*********************************************************************** + bool _initialized{false}; - void task_main(); //main task - static int task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_interval_perf; /**< loop interval performance counter */ void vehicle_cmd_poll(); - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output int parameters_update(); //Update local parameter cache void handle_command(); }; - -#endif diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp deleted file mode 100644 index 65cddf1dc20d..000000000000 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ /dev/null @@ -1,311 +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. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -static constexpr uint32_t SCHEDULE_INTERVAL{100_ms}; /**< The schedule interval in usec (10 Hz) */ - -using matrix::Dcmf; -using matrix::Quatf; -using matrix::Vector2f; -using matrix::Vector3f; - -class WindEstimatorModule : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - - WindEstimatorModule(); - - ~WindEstimatorModule(); - - /** @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); - - // run the main loop - void Run() override; - - int print_status() override; - -private: - - WindEstimator _wind_estimator; - orb_advert_t _wind_est_pub{nullptr}; /**< wind estimate topic */ - - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; - uORB::Subscription _param_sub{ORB_ID(parameter_update)}; - - perf_counter_t _perf_elapsed{}; - perf_counter_t _perf_interval{}; - - int _instance{-1}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_west_w_p_noise, - (ParamFloat) _param_west_sc_p_noise, - (ParamFloat) _param_west_tas_noise, - (ParamFloat) _param_west_beta_noise, - (ParamInt) _param_west_tas_gate, - (ParamInt) _param_west_beta_gate - ) - - int start(); - - void update_params(); - - bool subscribe_topics(); -}; - -WindEstimatorModule::WindEstimatorModule(): - ModuleParams(nullptr), - ScheduledWorkItem(px4::wq_configurations::lp_default) -{ - // initialise parameters - update_params(); - - _perf_elapsed = perf_alloc_once(PC_ELAPSED, "wind_estimator elapsed"); - _perf_interval = perf_alloc_once(PC_INTERVAL, "wind_estimator interval"); -} - -WindEstimatorModule::~WindEstimatorModule() -{ - ScheduleClear(); - - orb_unadvertise(_wind_est_pub); - - perf_free(_perf_elapsed); - perf_free(_perf_interval); -} - -int -WindEstimatorModule::task_spawn(int argc, char *argv[]) -{ - /* schedule a cycle to start things */ - WindEstimatorModule *dev = new WindEstimatorModule(); - - // check if the trampoline is called for the first time - if (!dev) { - PX4_ERR("alloc failed"); - return PX4_ERROR; - } - - _object.store(dev); - - if (dev) { - dev->ScheduleOnInterval(SCHEDULE_INTERVAL, 10000); - _task_id = task_id_is_work_queue; - return PX4_OK; - } - - return PX4_ERROR; -} - -void -WindEstimatorModule::Run() -{ - perf_count(_perf_interval); - perf_begin(_perf_elapsed); - - parameter_update_s param{}; - - if (_param_sub.update(¶m)) { - update_params(); - } - - bool lpos_valid = false; - bool att_valid = false; - - const hrt_abstime time_now_usec = hrt_absolute_time(); - - // validate required conditions for the filter to fuse measurements - - vehicle_attitude_s att{}; - - if (_vehicle_attitude_sub.copy(&att)) { - att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0); - } - - vehicle_local_position_s lpos{}; - - if (_vehicle_local_position_sub.copy(&lpos)) { - lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid; - } - - // update wind and airspeed estimator - _wind_estimator.update(time_now_usec); - - if (lpos_valid && att_valid) { - - Vector3f vI(lpos.vx, lpos.vy, lpos.vz); - Quatf q(att.q); - - // sideslip fusion - _wind_estimator.fuse_beta(time_now_usec, vI, q); - - // additionally, for airspeed fusion we need to have recent measurements - airspeed_s airspeed{}; - - if (_airspeed_sub.update(&airspeed)) { - if ((time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0)) { - const Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}}; - - // airspeed fusion - _wind_estimator.fuse_airspeed(time_now_usec, airspeed.true_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)}); - } - } - - // if we fused either airspeed or sideslip we publish a wind_estimate message - wind_estimate_s wind_est = {}; - - wind_est.timestamp = time_now_usec; - float wind[2]; - _wind_estimator.get_wind(wind); - wind_est.windspeed_north = wind[0]; - wind_est.windspeed_east = wind[1]; - float wind_cov[2]; - _wind_estimator.get_wind_var(wind_cov); - wind_est.variance_north = wind_cov[0]; - wind_est.variance_east = wind_cov[1]; - wind_est.tas_innov = _wind_estimator.get_tas_innov(); - wind_est.tas_innov_var = _wind_estimator.get_tas_innov_var(); - wind_est.beta_innov = _wind_estimator.get_beta_innov(); - wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var(); - wind_est.tas_scale = _wind_estimator.get_tas_scale(); - - orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &_instance, ORB_PRIO_DEFAULT); - } - - perf_end(_perf_elapsed); - - if (should_exit()) { - exit_and_cleanup(); - } -} - -void WindEstimatorModule::update_params() -{ - updateParams(); - - // update wind & airspeed scale estimator parameters - _wind_estimator.set_wind_p_noise(_param_west_w_p_noise.get()); - _wind_estimator.set_tas_scale_p_noise(_param_west_sc_p_noise.get()); - _wind_estimator.set_tas_noise(_param_west_tas_noise.get()); - _wind_estimator.set_beta_noise(_param_west_beta_noise.get()); - _wind_estimator.set_tas_gate(_param_west_tas_gate.get()); - _wind_estimator.set_beta_gate(_param_west_beta_gate.get()); -} - -int WindEstimatorModule::custom_command(int argc, char *argv[]) -{ - if (!is_running()) { - int ret = WindEstimatorModule::task_spawn(argc, argv); - - if (ret) { - return ret; - } - } - - return print_usage("unknown command"); -} - -int WindEstimatorModule::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -This module runs a combined wind and airspeed scale factor estimator. -If provided the vehicle NED speed and attitude it can estimate the horizontal wind components based on a zero -sideslip assumption. This makes the estimator only suitable for fixed wing vehicles. -If provided an airspeed measurement this module also estimates an airspeed scale factor based on the following model: -measured_airspeed = scale_factor * real_airspeed. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("wind_estimator", "estimator"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -int WindEstimatorModule::print_status() -{ - perf_print_counter(_perf_elapsed); - perf_print_counter(_perf_interval); - - if (_instance > -1) { - uORB::SubscriptionData est{ORB_ID(wind_estimate), (uint8_t)_instance}; - est.update(); - - print_message(est.get()); - } else { - PX4_INFO("Running, but never published"); - } - - return 0; -} - -extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]); - -int -wind_estimator_main(int argc, char *argv[]) -{ - return WindEstimatorModule::main(argc, argv); -} diff --git a/src/modules/wind_estimator/wind_estimator_params.c b/src/modules/wind_estimator/wind_estimator_params.c deleted file mode 100644 index eb4f71b0f987..000000000000 --- a/src/modules/wind_estimator/wind_estimator_params.c +++ /dev/null @@ -1,71 +0,0 @@ -/** - * Enable Wind estimator - * - * @boolean - * @reboot_required true - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_EN, 0); - -/** - * Wind estimator wind process noise. - * - * @min 0 - * @max 1 - * @unit m/s/s - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_W_P_NOISE, 0.1f); - -/** - * Wind estimator true airspeed scale process noise. - * - * @min 0 - * @max 0.1 - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_SC_P_NOISE, 0.0001); - -/** - * Wind estimator true airspeed measurement noise. - * - * @min 0 - * @max 4 - * @unit m/s - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_TAS_NOISE, 1.4); - -/** - * Wind estimator sideslip measurement noise. - * - * @min 0 - * @max 1 - * @unit rad - * @group Wind Estimator - */ -PARAM_DEFINE_FLOAT(WEST_BETA_NOISE, 0.3); - -/** - * Gate size for true airspeed fusion. - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @min 1 - * @max 5 - * @unit SD - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_TAS_GATE, 3); - -/** - * Gate size for true sideslip fusion. - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @min 1 - * @max 5 - * @unit SD - * @group Wind Estimator - */ -PARAM_DEFINE_INT32(WEST_BETA_GATE, 1); diff --git a/src/platforms/px4_common.h b/src/platforms/px4_common.h deleted file mode 100644 index 384b1bca656b..000000000000 --- a/src/platforms/px4_common.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once - -#ifdef __PX4_QURT -#include -size_t strnlen(const char *s, size_t maxlen); - -#endif diff --git a/src/platforms/px4_init.h b/src/platforms/px4_init.h index 77cebfadf45d..a0bf68a1d291 100644 --- a/src/platforms/px4_init.h +++ b/src/platforms/px4_init.h @@ -36,3 +36,19 @@ __BEGIN_DECLS int px4_platform_init(void); __END_DECLS + +#ifdef __cplusplus + +namespace px4 +{ + +/** + * Startup init method. It has no specific functionality, just prints a welcome + * message and sets the thread name + */ +__EXPORT void init(int argc, char *argv[], const char *process_name); + +} // namespace px4 + +#endif /* __cplusplus */ + diff --git a/src/platforms/px4_micro_hal.h b/src/platforms/px4_micro_hal.h index 62b8b9261e43..4bea4d458baf 100644 --- a/src/platforms/px4_micro_hal.h +++ b/src/platforms/px4_micro_hal.h @@ -31,199 +31,11 @@ * ****************************************************************************/ #pragma once + /* * This file is a shim to bridge to the many SoC architecture supported by PX4 */ -#ifdef __PX4_NUTTX -__BEGIN_DECLS -#include -#include - -/* For historical reasons (NuttX STM32 numbering) PX4 bus numbering is 1 based - * All PX4 code, including, board code is written to assuming 1 based numbering. - * The following macros are used to allow the board config to define the bus - * numbers in terms of the NuttX driver numbering. 1,2,3 for one based numbering - * schemes or 0,1,2 for zero based schemes. - */ - -#define PX4_BUS_NUMBER_TO_PX4(x) ((x)+PX4_BUS_OFFSET) /* Use to define Zero based to match Nuttx Driver but provide 1 based to PX4 */ -#define PX4_BUS_NUMBER_FROM_PX4(x) ((x)-PX4_BUS_OFFSET) /* Use to map PX4 1 based to NuttX driver 0 based */ - -# define px4_enter_critical_section() enter_critical_section() -# define px4_leave_critical_section(flags) leave_critical_section(flags) - -# if defined(CONFIG_ARCH_CHIP_STM32) || defined(CONFIG_ARCH_CHIP_STM32F7) - -# if defined(CONFIG_ARCH_CHIP_STM32) -# include -# define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F4 -# define PX4_FLASH_BASE STM32_FLASH_BASE -# if defined(CONFIG_STM32_STM32F4XXX) -# include -# define PX4_BBSRAM_SIZE STM32_BBSRAM_SIZE -# define PX4_BBSRAM_GETDESC_IOCTL STM32_BBSRAM_GETDESC_IOCTL -# endif -# define PX4_NUMBER_I2C_BUSES STM32_NI2C -# endif - -# if defined(CONFIG_ARCH_CHIP_STM32F7) -# define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_STM32F7 -# include -# include -void stm32_flash_lock(void); -void stm32_flash_unlock(void); -int stm32_flash_writeprotect(size_t page, bool enabled); -# 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 -# define PX4_FLASH_BASE 0x08000000 -# define PX4_NUMBER_I2C_BUSES STM32F7_NI2C -# endif - -# include -# include -# include - -/* STM32/32F7 defines the 96 bit UUID as - * init32_t[3] that can be read as bytes/half-words/words - * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 31:0 (offset 0) - * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 4) - * init32_t[2] PX4_CPU_UUID_ADDRESS[3] bits 96:64 (offset 8) - * - * The original PX4 stm32 (legacy) based implementation **displayed** the - * UUID as: ABCD EFGH IJKL - * Where: - * A was bit 31 and D was bit 0 - * E was bit 63 and H was bit 32 - * I was bit 95 and L was bit 64 - * - * Since the string was used by some manufactures to identify the units - * it must be preserved. - * - * For new targets moving forward we will use - * IJKL EFGH ABCD - */ -# 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) stm32_bbsram_savepanic(fileno, context, length) - -# define PX4_BUS_OFFSET 0 /* STM buses are 1 based no adjustment needed */ -# define px4_spibus_initialize(bus_num_1based) stm32_spibus_initialize(bus_num_1based) - -# define px4_i2cbus_initialize(bus_num_1based) stm32_i2cbus_initialize(bus_num_1based) -# define px4_i2cbus_uninitialize(pdev) stm32_i2cbus_uninitialize(pdev) - -# define px4_arch_configgpio(pinset) stm32_configgpio(pinset) -# define px4_arch_unconfiggpio(pinset) stm32_unconfiggpio(pinset) -# define px4_arch_gpioread(pinset) stm32_gpioread(pinset) -# define px4_arch_gpiowrite(pinset, value) stm32_gpiowrite(pinset, value) -# define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) stm32_gpiosetevent(pinset,r,f,e,fp,a) -#endif // defined(CONFIG_ARCH_CHIP_STM32) || defined(CONFIG_ARCH_CHIP_STM32F7) - -#if defined(CONFIG_ARCH_CHIP_KINETIS) -# define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_KINETISK66 - -# // Fixme: using ?? -# define PX4_BBSRAM_SIZE 2048 -# define PX4_BBSRAM_GETDESC_IOCTL 0 -# define PX4_NUMBER_I2C_BUSES KINETIS_NI2C - -# define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE -# define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO - -# include -# include -# include -# include - -/* Kinetis defines the 128 bit UUID as - * init32_t[4] that can be read as words - * init32_t[0] PX4_CPU_UUID_ADDRESS[0] bits 127:96 (offset 0) - * init32_t[1] PX4_CPU_UUID_ADDRESS[1] bits 95:64 (offset 4) - * init32_t[2] PX4_CPU_UUID_ADDRESS[1] bits 63:32 (offset 8) - * init32_t[3] PX4_CPU_UUID_ADDRESS[3] bits 31:0 (offset C) - * - * PX4 uses the words in bigendian order MSB to LSB - * word [0] [1] [2] [3] - * bits 127:96 95-64 63-32, 31-00, - */ -# define PX4_CPU_UUID_BYTE_LENGTH KINETIS_UID_SIZE -# 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 - -/* define common formating across all commands */ - -# define PX4_CPU_UUID_WORD32_FORMAT "%08x" -# define PX4_CPU_UUID_WORD32_SEPARATOR ":" - -# define PX4_CPU_UUID_WORD32_UNIQUE_H 3 /* Least significant digits change the most */ -# define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ -# define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ -# define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ - -/* 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 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 */ - -# define px4_spibus_initialize(bus_num_1based) kinetis_spibus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) - -# define px4_i2cbus_initialize(bus_num_1based) kinetis_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) -# define px4_i2cbus_uninitialize(pdev) kinetis_i2cbus_uninitialize(pdev) - -# define px4_arch_configgpio(pinset) kinetis_pinconfig(pinset) -# define px4_arch_unconfiggpio(pinset) -# define px4_arch_gpioread(pinset) kinetis_gpioread(pinset) -# define px4_arch_gpiowrite(pinset, value) kinetis_gpiowrite(pinset, value) - -/* kinetis_gpiosetevent is not implemented and will need to be added */ - -# define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) kinetis_gpiosetevent(pinset,r,f,e,fp,a) -# endif - -#include +// include arch-specific header +#include -__END_DECLS -#endif diff --git a/src/systemcmds/dmesg/CMakeLists.txt b/src/systemcmds/dmesg/CMakeLists.txt index 556ae6d37a80..089bddcb8d4a 100644 --- a/src/systemcmds/dmesg/CMakeLists.txt +++ b/src/systemcmds/dmesg/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE systemcmds__dmesg MAIN dmesg - STACK_MAIN 900 SRCS dmesg.cpp ) diff --git a/src/systemcmds/dumpfile/dumpfile.c b/src/systemcmds/dumpfile/dumpfile.c index 981c701b6dfe..8f2e798944d6 100644 --- a/src/systemcmds/dumpfile/dumpfile.c +++ b/src/systemcmds/dumpfile/dumpfile.c @@ -99,6 +99,7 @@ dumpfile_main(int argc, char *argv[]) if (tcsetattr(out, TCSANOW, &tc) < 0) { PX4_ERR("failed setting stdout attributes"); + fclose(f); return 1; } diff --git a/src/systemcmds/hardfault_log/CMakeLists.txt b/src/systemcmds/hardfault_log/CMakeLists.txt index c48b372839cf..fca7fc683929 100644 --- a/src/systemcmds/hardfault_log/CMakeLists.txt +++ b/src/systemcmds/hardfault_log/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE systemcmds__hardfault_log MAIN hardfault_log - STACK_MAIN 2100 COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS diff --git a/src/systemcmds/i2cdetect/i2cdetect.cpp b/src/systemcmds/i2cdetect/i2cdetect.cpp index e093012abc9e..9f038b94c9bd 100644 --- a/src/systemcmds/i2cdetect/i2cdetect.cpp +++ b/src/systemcmds/i2cdetect/i2cdetect.cpp @@ -83,14 +83,14 @@ int detect(int bus) px4_i2c_msg_t msgv[2]; // send - msgv[0].frequency = 1000000; + msgv[0].frequency = 100000; msgv[0].addr = addr; msgv[0].flags = 0; msgv[0].buffer = &send_data; msgv[0].length = sizeof(send_data); // recv - msgv[1].frequency = 1000000; + msgv[1].frequency = 100000; msgv[1].addr = addr; msgv[1].flags = I2C_M_READ; msgv[1].buffer = &recv_data;; diff --git a/src/systemcmds/led_control/CMakeLists.txt b/src/systemcmds/led_control/CMakeLists.txt index 46be8140e415..a7396160cce1 100644 --- a/src/systemcmds/led_control/CMakeLists.txt +++ b/src/systemcmds/led_control/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE systemcmds__led_control MAIN led_control - STACK_MAIN 2500 COMPILE_FLAGS SRCS led_control.cpp diff --git a/src/systemcmds/led_control/led_control.cpp b/src/systemcmds/led_control/led_control.cpp index 60e987c88b03..bd07fbb2cf1d 100644 --- a/src/systemcmds/led_control/led_control.cpp +++ b/src/systemcmds/led_control/led_control.cpp @@ -45,9 +45,10 @@ #include #include -static void usage(); +#include +#include -static orb_advert_t led_control_pub = nullptr; +static void usage(); extern "C" { __EXPORT int led_control_main(int argc, char *argv[]); @@ -57,12 +58,8 @@ static void publish_led_control(led_control_s &led_control) { led_control.timestamp = hrt_absolute_time(); - if (led_control_pub == nullptr) { - led_control_pub = orb_advertise_queue(ORB_ID(led_control), &led_control, LED_UORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(led_control), led_control_pub, &led_control); - } + uORB::PublicationQueued led_control_pub{ORB_ID(led_control)}; + led_control_pub.publish(led_control); } static void run_led_test1() diff --git a/src/systemcmds/motor_ramp/CMakeLists.txt b/src/systemcmds/motor_ramp/CMakeLists.txt index da92a7bf5a40..b5684aeb6dce 100644 --- a/src/systemcmds/motor_ramp/CMakeLists.txt +++ b/src/systemcmds/motor_ramp/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_module( MODULE systemcmds__motor_ramp MAIN motor_ramp - STACK_MAIN 1200 COMPILE_FLAGS -Wno-write-strings SRCS diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp index cf30b76b7c22..4d42026a4e50 100644 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,8 @@ static float _ramp_time; static int _min_pwm; static int _max_pwm; static Mode _mode; -static char *_mode_c; +static const char *_mode_c; +static const char *_pwm_output_dev = "/dev/pwm_output0"; /** * motor_ramp management function. @@ -124,16 +126,17 @@ Before starting, make sure to stop any running attitude controller: When starting, a background task is started, runs for several seconds (as specified), then exits. -Note: this command currently only supports the `/dev/pwm_output0` output. - ### Example -$ motor_ramp sine 1100 0.5 +$ motor_ramp sine -a 1100 -r 0.5 )DESCR_STR"); PRINT_MODULE_USAGE_NAME_SIMPLE("motor_ramp", "command"); PRINT_MODULE_USAGE_ARG("ramp|sine|square", "mode", false); - PRINT_MODULE_USAGE_ARG("